xref: /openbmc/linux/net/ipv6/ioam6.c (revision 12382d11)
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  
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  }
980