net/ipv6/ioam6.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  *  IPv6 IOAM implementation
   4  *
   5  *  Author:
   6  *  Justin Iurman <[email protected]>
   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 timespec64 ts;
 639         ktime_t tstamp;
 640         u64 raw64;
 641         u32 raw32;
 642         u16 raw16;
 643         u8 *data;
 644         u8 byte;
 645
 646         data = trace->data + trace->remlen * 4 - trace->nodelen * 4 - sclen * 4;
 647
 648         /* hop_lim and node_id */
 649         if (trace->type.bit0) {
 650                 byte = ipv6_hdr(skb)->hop_limit;
 651                 if (is_input)
 652                         byte--;
 653
 654                 raw32 = dev_net(skb_dst(skb)->dev)->ipv6.sysctl.ioam6_id;
 655
 656                 *(__be32 *)data = cpu_to_be32((byte << 24) | raw32);
 657                 data += sizeof(__be32);
 658         }
 659
 660         /* ingress_if_id and egress_if_id */
 661         if (trace->type.bit1) {
 662                 if (!skb->dev)
 663                         raw16 = IOAM6_U16_UNAVAILABLE;
 664                 else
 665                         raw16 = (__force u16)__in6_dev_get(skb->dev)->cnf.ioam6_id;
 666
 667                 *(__be16 *)data = cpu_to_be16(raw16);
 668                 data += sizeof(__be16);
 669
 670                 if (skb_dst(skb)->dev->flags & IFF_LOOPBACK)
 671                         raw16 = IOAM6_U16_UNAVAILABLE;
 672                 else
 673                         raw16 = (__force u16)__in6_dev_get(skb_dst(skb)->dev)->cnf.ioam6_id;
 674
 675                 *(__be16 *)data = cpu_to_be16(raw16);
 676                 data += sizeof(__be16);
 677         }
 678
 679         /* timestamp seconds */
 680         if (trace->type.bit2) {
 681                 if (!skb->dev) {
 682                         *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 683                 } else {
 684                         tstamp = skb_tstamp_cond(skb, true);
 685                         ts = ktime_to_timespec64(tstamp);
 686
 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 (!trace->type.bit2) {
 698                                 tstamp = skb_tstamp_cond(skb, true);
 699                                 ts = ktime_to_timespec64(tstamp);
 700                         }
 701
 702                         *(__be32 *)data = cpu_to_be32((u32)(ts.tv_nsec / NSEC_PER_USEC));
 703                 }
 704                 data += sizeof(__be32);
 705         }
 706
 707         /* transit delay */
 708         if (trace->type.bit4) {
 709                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 710                 data += sizeof(__be32);
 711         }
 712
 713         /* namespace data */
 714         if (trace->type.bit5) {
 715                 *(__be32 *)data = ns->data;
 716                 data += sizeof(__be32);
 717         }
 718
 719         /* queue depth */
 720         if (trace->type.bit6) {
 721                 struct netdev_queue *queue;
 722                 struct Qdisc *qdisc;
 723                 __u32 qlen, backlog;
 724
 725                 if (skb_dst(skb)->dev->flags & IFF_LOOPBACK) {
 726                         *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 727                 } else {
 728                         queue = skb_get_tx_queue(skb_dst(skb)->dev, skb);
 729                         qdisc = rcu_dereference(queue->qdisc);
 730                         qdisc_qstats_qlen_backlog(qdisc, &qlen, &backlog);
 731
 732                         *(__be32 *)data = cpu_to_be32(backlog);
 733                 }
 734                 data += sizeof(__be32);
 735         }
 736
 737         /* checksum complement */
 738         if (trace->type.bit7) {
 739                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 740                 data += sizeof(__be32);
 741         }
 742
 743         /* hop_lim and node_id (wide) */
 744         if (trace->type.bit8) {
 745                 byte = ipv6_hdr(skb)->hop_limit;
 746                 if (is_input)
 747                         byte--;
 748
 749                 raw64 = dev_net(skb_dst(skb)->dev)->ipv6.sysctl.ioam6_id_wide;
 750
 751                 *(__be64 *)data = cpu_to_be64(((u64)byte << 56) | raw64);
 752                 data += sizeof(__be64);
 753         }
 754
 755         /* ingress_if_id and egress_if_id (wide) */
 756         if (trace->type.bit9) {
 757                 if (!skb->dev)
 758                         raw32 = IOAM6_U32_UNAVAILABLE;
 759                 else
 760                         raw32 = __in6_dev_get(skb->dev)->cnf.ioam6_id_wide;
 761
 762                 *(__be32 *)data = cpu_to_be32(raw32);
 763                 data += sizeof(__be32);
 764
 765                 if (skb_dst(skb)->dev->flags & IFF_LOOPBACK)
 766                         raw32 = IOAM6_U32_UNAVAILABLE;
 767                 else
 768                         raw32 = __in6_dev_get(skb_dst(skb)->dev)->cnf.ioam6_id_wide;
 769
 770                 *(__be32 *)data = cpu_to_be32(raw32);
 771                 data += sizeof(__be32);
 772         }
 773
 774         /* namespace data (wide) */
 775         if (trace->type.bit10) {
 776                 *(__be64 *)data = ns->data_wide;
 777                 data += sizeof(__be64);
 778         }
 779
 780         /* buffer occupancy */
 781         if (trace->type.bit11) {
 782                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 783                 data += sizeof(__be32);
 784         }
 785
 786         /* bit12 undefined: filled with empty value */
 787         if (trace->type.bit12) {
 788                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 789                 data += sizeof(__be32);
 790         }
 791
 792         /* bit13 undefined: filled with empty value */
 793         if (trace->type.bit13) {
 794                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 795                 data += sizeof(__be32);
 796         }
 797
 798         /* bit14 undefined: filled with empty value */
 799         if (trace->type.bit14) {
 800                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 801                 data += sizeof(__be32);
 802         }
 803
 804         /* bit15 undefined: filled with empty value */
 805         if (trace->type.bit15) {
 806                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 807                 data += sizeof(__be32);
 808         }
 809
 810         /* bit16 undefined: filled with empty value */
 811         if (trace->type.bit16) {
 812                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 813                 data += sizeof(__be32);
 814         }
 815
 816         /* bit17 undefined: filled with empty value */
 817         if (trace->type.bit17) {
 818                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 819                 data += sizeof(__be32);
 820         }
 821
 822         /* bit18 undefined: filled with empty value */
 823         if (trace->type.bit18) {
 824                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 825                 data += sizeof(__be32);
 826         }
 827
 828         /* bit19 undefined: filled with empty value */
 829         if (trace->type.bit19) {
 830                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 831                 data += sizeof(__be32);
 832         }
 833
 834         /* bit20 undefined: filled with empty value */
 835         if (trace->type.bit20) {
 836                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 837                 data += sizeof(__be32);
 838         }
 839
 840         /* bit21 undefined: filled with empty value */
 841         if (trace->type.bit21) {
 842                 *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 843                 data += sizeof(__be32);
 844         }
 845
 846         /* opaque state snapshot */
 847         if (trace->type.bit22) {
 848                 if (!sc) {
 849                         *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE >> 8);
 850                 } else {
 851                         *(__be32 *)data = sc->hdr;
 852                         data += sizeof(__be32);
 853
 854                         memcpy(data, sc->data, sc->len);
 855                 }
 856         }
 857 }
 858
 859 /* called with rcu_read_lock() */
 860 void ioam6_fill_trace_data(struct sk_buff *skb,
 861                            struct ioam6_namespace *ns,
 862                            struct ioam6_trace_hdr *trace,
 863                            bool is_input)
 864 {
 865         struct ioam6_schema *sc;
 866         u8 sclen = 0;
 867
 868         /* Skip if Overflow flag is set
 869          */
 870         if (trace->overflow)
 871                 return;
 872
 873         /* NodeLen does not include Opaque State Snapshot length. We need to
 874          * take it into account if the corresponding bit is set (bit 22) and
 875          * if the current IOAM namespace has an active schema attached to it
 876          */
 877         sc = rcu_dereference(ns->schema);
 878         if (trace->type.bit22) {
 879                 sclen = sizeof_field(struct ioam6_schema, hdr) / 4;
 880
 881                 if (sc)
 882                         sclen += sc->len / 4;
 883         }
 884
 885         /* If there is no space remaining, we set the Overflow flag and we
 886          * skip without filling the trace
 887          */
 888         if (!trace->remlen || trace->remlen < trace->nodelen + sclen) {
 889                 trace->overflow = 1;
 890                 return;
 891         }
 892
 893         __ioam6_fill_trace_data(skb, ns, trace, sc, sclen, is_input);
 894         trace->remlen -= trace->nodelen + sclen;
 895 }
 896
 897 static int __net_init ioam6_net_init(struct net *net)
 898 {
 899         struct ioam6_pernet_data *nsdata;
 900         int err = -ENOMEM;
 901
 902         nsdata = kzalloc(sizeof(*nsdata), GFP_KERNEL);
 903         if (!nsdata)
 904                 goto out;
 905
 906         mutex_init(&nsdata->lock);
 907         net->ipv6.ioam6_data = nsdata;
 908
 909         err = rhashtable_init(&nsdata->namespaces, &rht_ns_params);
 910         if (err)
 911                 goto free_nsdata;
 912
 913         err = rhashtable_init(&nsdata->schemas, &rht_sc_params);
 914         if (err)
 915                 goto free_rht_ns;
 916
 917 out:
 918         return err;
 919 free_rht_ns:
 920         rhashtable_destroy(&nsdata->namespaces);
 921 free_nsdata:
 922         kfree(nsdata);
 923         net->ipv6.ioam6_data = NULL;
 924         goto out;
 925 }
 926
 927 static void __net_exit ioam6_net_exit(struct net *net)
 928 {
 929         struct ioam6_pernet_data *nsdata = ioam6_pernet(net);
 930
 931         rhashtable_free_and_destroy(&nsdata->namespaces, ioam6_free_ns, NULL);
 932         rhashtable_free_and_destroy(&nsdata->schemas, ioam6_free_sc, NULL);
 933
 934         kfree(nsdata);
 935 }
 936
 937 static struct pernet_operations ioam6_net_ops = {
 938         .init = ioam6_net_init,
 939         .exit = ioam6_net_exit,
 940 };
 941
 942 int __init ioam6_init(void)
 943 {
 944         int err = register_pernet_subsys(&ioam6_net_ops);
 945         if (err)
 946                 goto out;
 947
 948         err = genl_register_family(&ioam6_genl_family);
 949         if (err)
 950                 goto out_unregister_pernet_subsys;
 951
 952 #ifdef CONFIG_IPV6_IOAM6_LWTUNNEL
 953         err = ioam6_iptunnel_init();
 954         if (err)
 955                 goto out_unregister_genl;
 956 #endif
 957
 958         pr_info("In-situ OAM (IOAM) with IPv6\n");
 959
 960 out:
 961         return err;
 962 #ifdef CONFIG_IPV6_IOAM6_LWTUNNEL
 963 out_unregister_genl:
 964         genl_unregister_family(&ioam6_genl_family);
 965 #endif
 966 out_unregister_pernet_subsys:
 967         unregister_pernet_subsys(&ioam6_net_ops);
 968         goto out;
 969 }
 970
 971 void ioam6_exit(void)
 972 {
 973 #ifdef CONFIG_IPV6_IOAM6_LWTUNNEL
 974         ioam6_iptunnel_exit();
 975 #endif
 976         genl_unregister_family(&ioam6_genl_family);
 977         unregister_pernet_subsys(&ioam6_net_ops);
 978 }