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	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
156out_unlock:
157	mutex_unlock(&nsdata->lock);
158	return err;
159}
160
161static 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
196out_unlock:
197	mutex_unlock(&nsdata->lock);
198	return err;
199}
200
201static 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
241nla_put_failure:
242	genlmsg_cancel(skb, hdr);
243	return -EMSGSIZE;
244}
245
246static 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
264static 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
274static 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
307done:
308	rhashtable_walk_stop(iter);
309	return err;
310}
311
312static 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
352out_unlock:
353	mutex_unlock(&nsdata->lock);
354	return err;
355free_sc:
356	kfree(sc);
357	goto out_unlock;
358}
359
360static 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
394out_unlock:
395	mutex_unlock(&nsdata->lock);
396	return err;
397}
398
399static 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
427nla_put_failure:
428	genlmsg_cancel(skb, hdr);
429	return -EMSGSIZE;
430}
431
432static 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
450static 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
460static 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
493done:
494	rhashtable_walk_stop(iter);
495	return err;
496}
497
498static 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
551out_unlock:
552	mutex_unlock(&nsdata->lock);
553	return err;
554}
555
556static 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
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
615static 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
626struct 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
633static 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() */
861void 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
898static 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
918out:
919	return err;
920free_rht_ns:
921	rhashtable_destroy(&nsdata->namespaces);
922free_nsdata:
923	kfree(nsdata);
924	net->ipv6.ioam6_data = NULL;
925	goto out;
926}
927
928static 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
938static struct pernet_operations ioam6_net_ops = {
939	.init = ioam6_net_init,
940	.exit = ioam6_net_exit,
941};
942
943int __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
961out:
962	return err;
963#ifdef CONFIG_IPV6_IOAM6_LWTUNNEL
964out_unregister_genl:
965	genl_unregister_family(&ioam6_genl_family);
966#endif
967out_unregister_pernet_subsys:
968	unregister_pernet_subsys(&ioam6_net_ops);
969	goto out;
970}
971
972void 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}