ioam6.c

   1// SPDX-License-Identifier: GPL-2.0+
   2/*
   3 *  IPv6 IOAM implementation
   4 *
   5 *  Author:
   6 *  Justin Iurman <justin.iurman@uliege.be>
   7 */
   8
   9#include <linux/errno.h>
  10#include <linux/types.h>
  11#include <linux/kernel.h>
  12#include <linux/net.h>
  13#include <linux/ioam6.h>
  14#include <linux/ioam6_genl.h>
  15#include <linux/rhashtable.h>
  16#include <linux/netdevice.h>
  17
  18#include <net/addrconf.h>
  19#include <net/genetlink.h>
  20#include <net/ioam6.h>
  21#include <net/sch_generic.h>
  22
  23static void ioam6_ns_release(struct ioam6_namespace *ns)
  24{
  25	kfree_rcu(ns, rcu);
  26}
  27
  28static void ioam6_sc_release(struct ioam6_schema *sc)
  29{
  30	kfree_rcu(sc, rcu);
  31}
  32
  33static 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
  41static 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
  49static 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
  56static 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
  63static 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
  71static 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
  79static struct genl_family ioam6_genl_family;
  80
  81static 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
  87static const struct nla_policy ioam6_genl_policy_delns[] = {
  88	[IOAM6_ATTR_NS_ID]	= { .type = NLA_U16 },
  89};
  90
  91static 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
  97static const struct nla_policy ioam6_genl_policy_delsc[] = {
  98	[IOAM6_ATTR_SC_ID]	= { .type = NLA_U32 },
  99};
 100
 101static 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
 107static 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
 152out_unlock:
 153	mutex_unlock(&nsdata->lock);
 154	return err;
 155}
 156
 157static 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
 192out_unlock:
 193	mutex_unlock(&nsdata->lock);
 194	return err;
 195}
 196
 197static 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
 237nla_put_failure:
 238	genlmsg_cancel(skb, hdr);
 239	return -EMSGSIZE;
 240}
 241
 242static 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
 260static 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
 270static 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
 303done:
 304	rhashtable_walk_stop(iter);
 305	return err;
 306}
 307
 308static 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
 348out_unlock:
 349	mutex_unlock(&nsdata->lock);
 350	return err;
 351free_sc:
 352	kfree(sc);
 353	goto out_unlock;
 354}
 355
 356static 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
 390out_unlock:
 391	mutex_unlock(&nsdata->lock);
 392	return err;
 393}
 394
 395static 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
 423nla_put_failure:
 424	genlmsg_cancel(skb, hdr);
 425	return -EMSGSIZE;
 426}
 427
 428static 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
 446static 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
 456static 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
 489done:
 490	rhashtable_walk_stop(iter);
 491	return err;
 492}
 493
 494static 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
 547out_unlock:
 548	mutex_unlock(&nsdata->lock);
 549	return err;
 550}
 551
 552static 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
 613static 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
 618static 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
 635void 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
 669nla_put_failure:
 670	nlmsg_free(skb);
 671}
 672
 673static 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
 686struct 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#define IOAM6_MASK_SHORT_FIELDS 0xff1ffc00
 694#define IOAM6_MASK_WIDE_FIELDS  0x00e00000
 695
 696u8 ioam6_trace_compute_nodelen(u32 trace_type)
 697{
 698	u8 nodelen = hweight32(trace_type & IOAM6_MASK_SHORT_FIELDS)
 699				* (sizeof(__be32) / 4);
 700
 701	nodelen += hweight32(trace_type & IOAM6_MASK_WIDE_FIELDS)
 702				* (sizeof(__be64) / 4);
 703
 704	return nodelen;
 705}
 706
 707static void __ioam6_fill_trace_data(struct sk_buff *skb,
 708				    struct ioam6_namespace *ns,
 709				    struct ioam6_trace_hdr *trace,
 710				    struct ioam6_schema *sc,
 711				    unsigned int sclen, bool is_input)
 712{
 713	/* Note: skb_dst_dev_rcu() can't be NULL at this point. */
 714	struct net_device *dev = skb_dst_dev_rcu(skb);
 715	struct inet6_dev *i_skb_dev, *idev;
 716	struct timespec64 ts;
 717	ktime_t tstamp;
 718	u64 raw64;
 719	u32 raw32;
 720	u16 raw16;
 721	u8 *data;
 722	u8 byte;
 723
 724	data = trace->data + trace->remlen * 4 - trace->nodelen * 4 - sclen * 4;
 725
 726	i_skb_dev = skb->dev ? __in6_dev_get(skb->dev) : NULL;
 727	idev = __in6_dev_get(dev);
 728
 729	/* hop_lim and node_id */
 730	if (trace->type.bit0) {
 731		byte = ipv6_hdr(skb)->hop_limit;
 732		if (is_input)
 733			byte--;
 734
 735		raw32 = READ_ONCE(dev_net(dev)->ipv6.sysctl.ioam6_id);
 736
 737		*(__be32 *)data = cpu_to_be32((byte << 24) | raw32);
 738		data += sizeof(__be32);
 739	}
 740
 741	/* ingress_if_id and egress_if_id */
 742	if (trace->type.bit1) {
 743		if (!i_skb_dev)
 744			raw16 = IOAM6_U16_UNAVAILABLE;
 745		else
 746			raw16 = (__force u16)READ_ONCE(i_skb_dev->cnf.ioam6_id);
 747
 748		*(__be16 *)data = cpu_to_be16(raw16);
 749		data += sizeof(__be16);
 750
 751		if ((dev->flags & IFF_LOOPBACK) || !idev)
 752			raw16 = IOAM6_U16_UNAVAILABLE;
 753		else
 754			raw16 = (__force u16)READ_ONCE(idev->cnf.ioam6_id);
 755
 756		*(__be16 *)data = cpu_to_be16(raw16);
 757		data += sizeof(__be16);
 758	}
 759
 760	/* timestamp seconds */
 761	if (trace->type.bit2) {
 762		if (!skb->dev) {
 763			*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 764		} else {
 765			tstamp = skb_tstamp_cond(skb, true);
 766			ts = ktime_to_timespec64(tstamp);
 767
 768			*(__be32 *)data = cpu_to_be32((u32)ts.tv_sec);
 769		}
 770		data += sizeof(__be32);
 771	}
 772
 773	/* timestamp subseconds */
 774	if (trace->type.bit3) {
 775		if (!skb->dev) {
 776			*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 777		} else {
 778			if (!trace->type.bit2) {
 779				tstamp = skb_tstamp_cond(skb, true);
 780				ts = ktime_to_timespec64(tstamp);
 781			}
 782
 783			*(__be32 *)data = cpu_to_be32((u32)(ts.tv_nsec / NSEC_PER_USEC));
 784		}
 785		data += sizeof(__be32);
 786	}
 787
 788	/* transit delay */
 789	if (trace->type.bit4) {
 790		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 791		data += sizeof(__be32);
 792	}
 793
 794	/* namespace data */
 795	if (trace->type.bit5) {
 796		*(__be32 *)data = ns->data;
 797		data += sizeof(__be32);
 798	}
 799
 800	/* queue depth */
 801	if (trace->type.bit6) {
 802		struct netdev_queue *queue;
 803		struct Qdisc *qdisc;
 804		__u32 qlen, backlog;
 805
 806		if (dev->flags & IFF_LOOPBACK ||
 807		    skb_get_queue_mapping(skb) >= dev->num_tx_queues) {
 808			*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 809		} else {
 810			queue = skb_get_tx_queue(dev, skb);
 811			qdisc = rcu_dereference(queue->qdisc);
 812
 813			spin_lock_bh(qdisc_lock(qdisc));
 814			qdisc_qstats_qlen_backlog(qdisc, &qlen, &backlog);
 815			spin_unlock_bh(qdisc_lock(qdisc));
 816
 817			*(__be32 *)data = cpu_to_be32(backlog);
 818		}
 819		data += sizeof(__be32);
 820	}
 821
 822	/* checksum complement */
 823	if (trace->type.bit7) {
 824		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 825		data += sizeof(__be32);
 826	}
 827
 828	/* hop_lim and node_id (wide) */
 829	if (trace->type.bit8) {
 830		byte = ipv6_hdr(skb)->hop_limit;
 831		if (is_input)
 832			byte--;
 833
 834		raw64 = READ_ONCE(dev_net(dev)->ipv6.sysctl.ioam6_id_wide);
 835
 836		*(__be64 *)data = cpu_to_be64(((u64)byte << 56) | raw64);
 837		data += sizeof(__be64);
 838	}
 839
 840	/* ingress_if_id and egress_if_id (wide) */
 841	if (trace->type.bit9) {
 842		if (!i_skb_dev)
 843			raw32 = IOAM6_U32_UNAVAILABLE;
 844		else
 845			raw32 = READ_ONCE(i_skb_dev->cnf.ioam6_id_wide);
 846
 847		*(__be32 *)data = cpu_to_be32(raw32);
 848		data += sizeof(__be32);
 849
 850		if ((dev->flags & IFF_LOOPBACK) || !idev)
 851			raw32 = IOAM6_U32_UNAVAILABLE;
 852		else
 853			raw32 = READ_ONCE(idev->cnf.ioam6_id_wide);
 854
 855		*(__be32 *)data = cpu_to_be32(raw32);
 856		data += sizeof(__be32);
 857	}
 858
 859	/* namespace data (wide) */
 860	if (trace->type.bit10) {
 861		*(__be64 *)data = ns->data_wide;
 862		data += sizeof(__be64);
 863	}
 864
 865	/* buffer occupancy */
 866	if (trace->type.bit11) {
 867		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 868		data += sizeof(__be32);
 869	}
 870
 871	/* bit12 undefined: filled with empty value */
 872	if (trace->type.bit12) {
 873		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 874		data += sizeof(__be32);
 875	}
 876
 877	/* bit13 undefined: filled with empty value */
 878	if (trace->type.bit13) {
 879		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 880		data += sizeof(__be32);
 881	}
 882
 883	/* bit14 undefined: filled with empty value */
 884	if (trace->type.bit14) {
 885		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 886		data += sizeof(__be32);
 887	}
 888
 889	/* bit15 undefined: filled with empty value */
 890	if (trace->type.bit15) {
 891		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 892		data += sizeof(__be32);
 893	}
 894
 895	/* bit16 undefined: filled with empty value */
 896	if (trace->type.bit16) {
 897		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 898		data += sizeof(__be32);
 899	}
 900
 901	/* bit17 undefined: filled with empty value */
 902	if (trace->type.bit17) {
 903		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 904		data += sizeof(__be32);
 905	}
 906
 907	/* bit18 undefined: filled with empty value */
 908	if (trace->type.bit18) {
 909		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 910		data += sizeof(__be32);
 911	}
 912
 913	/* bit19 undefined: filled with empty value */
 914	if (trace->type.bit19) {
 915		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 916		data += sizeof(__be32);
 917	}
 918
 919	/* bit20 undefined: filled with empty value */
 920	if (trace->type.bit20) {
 921		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 922		data += sizeof(__be32);
 923	}
 924
 925	/* bit21 undefined: filled with empty value */
 926	if (trace->type.bit21) {
 927		*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE);
 928		data += sizeof(__be32);
 929	}
 930
 931	/* opaque state snapshot */
 932	if (trace->type.bit22) {
 933		if (!sc) {
 934			*(__be32 *)data = cpu_to_be32(IOAM6_U32_UNAVAILABLE >> 8);
 935		} else {
 936			*(__be32 *)data = sc->hdr;
 937			data += sizeof(__be32);
 938
 939			memcpy(data, sc->data, sc->len);
 940		}
 941	}
 942}
 943
 944/* called with rcu_read_lock() */
 945void ioam6_fill_trace_data(struct sk_buff *skb,
 946			   struct ioam6_namespace *ns,
 947			   struct ioam6_trace_hdr *trace,
 948			   bool is_input)
 949{
 950	struct ioam6_schema *sc;
 951	unsigned int sclen = 0;
 952
 953	/* Skip if Overflow flag is set
 954	 */
 955	if (trace->overflow)
 956		return;
 957
 958	/* NodeLen does not include Opaque State Snapshot length. We need to
 959	 * take it into account if the corresponding bit is set (bit 22) and
 960	 * if the current IOAM namespace has an active schema attached to it
 961	 */
 962	sc = rcu_dereference(ns->schema);
 963	if (trace->type.bit22) {
 964		sclen = sizeof_field(struct ioam6_schema, hdr) / 4;
 965
 966		if (sc)
 967			sclen += sc->len / 4;
 968	}
 969
 970	/* If there is no space remaining, we set the Overflow flag and we
 971	 * skip without filling the trace
 972	 */
 973	if (!trace->remlen || trace->remlen < trace->nodelen + sclen) {
 974		trace->overflow = 1;
 975		return;
 976	}
 977
 978	__ioam6_fill_trace_data(skb, ns, trace, sc, sclen, is_input);
 979	trace->remlen -= trace->nodelen + sclen;
 980}
 981
 982static int __net_init ioam6_net_init(struct net *net)
 983{
 984	struct ioam6_pernet_data *nsdata;
 985	int err = -ENOMEM;
 986
 987	nsdata = kzalloc(sizeof(*nsdata), GFP_KERNEL);
 988	if (!nsdata)
 989		goto out;
 990
 991	mutex_init(&nsdata->lock);
 992	net->ipv6.ioam6_data = nsdata;
 993
 994	err = rhashtable_init(&nsdata->namespaces, &rht_ns_params);
 995	if (err)
 996		goto free_nsdata;
 997
 998	err = rhashtable_init(&nsdata->schemas, &rht_sc_params);
 999	if (err)
1000		goto free_rht_ns;
1001
1002out:
1003	return err;
1004free_rht_ns:
1005	rhashtable_destroy(&nsdata->namespaces);
1006free_nsdata:
1007	kfree(nsdata);
1008	net->ipv6.ioam6_data = NULL;
1009	goto out;
1010}
1011
1012static void __net_exit ioam6_net_exit(struct net *net)
1013{
1014	struct ioam6_pernet_data *nsdata = ioam6_pernet(net);
1015
1016	rhashtable_free_and_destroy(&nsdata->namespaces, ioam6_free_ns, NULL);
1017	rhashtable_free_and_destroy(&nsdata->schemas, ioam6_free_sc, NULL);
1018
1019	kfree(nsdata);
1020}
1021
1022static struct pernet_operations ioam6_net_ops = {
1023	.init = ioam6_net_init,
1024	.exit = ioam6_net_exit,
1025};
1026
1027int __init ioam6_init(void)
1028{
1029	int err = register_pernet_subsys(&ioam6_net_ops);
1030	if (err)
1031		goto out;
1032
1033	err = genl_register_family(&ioam6_genl_family);
1034	if (err)
1035		goto out_unregister_pernet_subsys;
1036
1037#ifdef CONFIG_IPV6_IOAM6_LWTUNNEL
1038	err = ioam6_iptunnel_init();
1039	if (err)
1040		goto out_unregister_genl;
1041#endif
1042
1043	pr_info("In-situ OAM (IOAM) with IPv6\n");
1044
1045out:
1046	return err;
1047#ifdef CONFIG_IPV6_IOAM6_LWTUNNEL
1048out_unregister_genl:
1049	genl_unregister_family(&ioam6_genl_family);
1050#endif
1051out_unregister_pernet_subsys:
1052	unregister_pernet_subsys(&ioam6_net_ops);
1053	goto out;
1054}
1055
1056void ioam6_exit(void)
1057{
1058#ifdef CONFIG_IPV6_IOAM6_LWTUNNEL
1059	ioam6_iptunnel_exit();
1060#endif
1061	genl_unregister_family(&ioam6_genl_family);
1062	unregister_pernet_subsys(&ioam6_net_ops);
1063}