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         .resv_start_op  = IOAM6_CMD_NS_SET_SCHEMA + 1,
 623         .module         = THIS_MODULE,
 624 };
 625
 626 struct ioam6_namespace *ioam6_namespace(struct net *net, __be16 id)
 627 {
 628         struct ioam6_pernet_data *nsdata = ioam6_pernet(net);
 629
 630         return rhashtable_lookup_fast(&nsdata->namespaces, &id, rht_ns_params);
 631 }
 632
 633 static void __ioam6_fill_trace_data(struct sk_buff *skb,
 634                                     struct ioam6_namespace *ns,
 635                                     struct ioam6_trace_hdr *trace,
 636                                     struct ioam6_schema *sc,
 637                                     u8 sclen, bool is_input)
 638 {
 639         struct timespec64 ts;
 640         ktime_t tstamp;
 641         u64 raw64;
 642         u32 raw32;
 643         u16 raw16;
 644         u8 *data;
 645         u8 byte;
 646
 647         data = trace->data + trace->remlen * 4 - trace->nodelen * 4 - sclen * 4;
 648
 649         /* hop_lim and node_id */
 650         if (trace->type.bit0) {
 651                 byte = ipv6_hdr(skb)->hop_limit;
 652                 if (is_input)
 653                         byte--;
 654
 655                 raw32 = dev_net(skb_dst(skb)->dev)->ipv6.sysctl.ioam6_id;
 656
 657                 *(__be32 *)data = cpu_to_be32((byte << 24) | raw32);
 658                 data += sizeof(__be32);
 659         }
 660
 661         /* ingress_if_id and egress_if_id */
 662         if (trace->type.bit1) {
 663                 if (!skb->dev)
 664                         raw16 = IOAM6_U16_UNAVAILABLE;
 665                 else
 666                         raw16 = (__force u16)__in6_dev_get(skb->dev)->cnf.ioam6_id;
 667
 668                 *(__be16 *)data = cpu_to_be16(raw16);
 669                 data += sizeof(__be16);
 670
 671                 if (skb_dst(skb)->dev->flags & IFF_LOOPBACK)
 672                         raw16 = IOAM6_U16_UNAVAILABLE;
 673                 else
 674                         raw16 = (__force u16)__in6_dev_get(skb_dst(skb)->dev)->cnf.ioam6_id;
 675
 676                 *(__be16 *)data = cpu_to_be16(raw16);
 677                 data += sizeof(__be16);
 678         }
 679
 680         /* timestamp seconds */
 681         if (trace->type.bit2) {
 682                 if (!skb->dev) {
 683                         *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 684                 } else {
 685                         tstamp = skb_tstamp_cond(skb, true);
 686                         ts = ktime_to_timespec64(tstamp);
 687
 688                         *(__be32 *)data = cpu_to_be32((u32)ts.tv_sec);
 689                 }
 690                 data += sizeof(__be32);
 691         }
 692
 693         /* timestamp subseconds */
 694         if (trace->type.bit3) {
 695                 if (!skb->dev) {
 696                         *(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 697                 } else {
 698                         if (!trace->type.bit2) {
 699                                 tstamp = skb_tstamp_cond(skb, true);
 700                                 ts = ktime_to_timespec64(tstamp);
 701                         }
 702
 703                         *(__be32 *)data = cpu_to_be32((u32)(ts.tv_nsec / NSEC_PER_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 }