Loading...
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}
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
693static 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() */
921void 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
958static 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
978out:
979 return err;
980free_rht_ns:
981 rhashtable_destroy(&nsdata->namespaces);
982free_nsdata:
983 kfree(nsdata);
984 net->ipv6.ioam6_data = NULL;
985 goto out;
986}
987
988static 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
998static struct pernet_operations ioam6_net_ops = {
999 .init = ioam6_net_init,
1000 .exit = ioam6_net_exit,
1001};
1002
1003int __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
1021out:
1022 return err;
1023#ifdef CONFIG_IPV6_IOAM6_LWTUNNEL
1024out_unregister_genl:
1025 genl_unregister_family(&ioam6_genl_family);
1026#endif
1027out_unregister_pernet_subsys:
1028 unregister_pernet_subsys(&ioam6_net_ops);
1029 goto out;
1030}
1031
1032void 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}