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