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