WIP FPC-III support
[linux/fpc-iii.git] / net / sched / act_gate.c
bloba78cb79657182d6e1a87424f715cc151f29176f7
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /* Copyright 2020 NXP */
4 #include <linux/module.h>
5 #include <linux/types.h>
6 #include <linux/kernel.h>
7 #include <linux/string.h>
8 #include <linux/errno.h>
9 #include <linux/skbuff.h>
10 #include <linux/rtnetlink.h>
11 #include <linux/init.h>
12 #include <linux/slab.h>
13 #include <net/act_api.h>
14 #include <net/netlink.h>
15 #include <net/pkt_cls.h>
16 #include <net/tc_act/tc_gate.h>
18 static unsigned int gate_net_id;
19 static struct tc_action_ops act_gate_ops;
21 static ktime_t gate_get_time(struct tcf_gate *gact)
23 ktime_t mono = ktime_get();
25 switch (gact->tk_offset) {
26 case TK_OFFS_MAX:
27 return mono;
28 default:
29 return ktime_mono_to_any(mono, gact->tk_offset);
32 return KTIME_MAX;
35 static void gate_get_start_time(struct tcf_gate *gact, ktime_t *start)
37 struct tcf_gate_params *param = &gact->param;
38 ktime_t now, base, cycle;
39 u64 n;
41 base = ns_to_ktime(param->tcfg_basetime);
42 now = gate_get_time(gact);
44 if (ktime_after(base, now)) {
45 *start = base;
46 return;
49 cycle = param->tcfg_cycletime;
51 n = div64_u64(ktime_sub_ns(now, base), cycle);
52 *start = ktime_add_ns(base, (n + 1) * cycle);
55 static void gate_start_timer(struct tcf_gate *gact, ktime_t start)
57 ktime_t expires;
59 expires = hrtimer_get_expires(&gact->hitimer);
60 if (expires == 0)
61 expires = KTIME_MAX;
63 start = min_t(ktime_t, start, expires);
65 hrtimer_start(&gact->hitimer, start, HRTIMER_MODE_ABS_SOFT);
68 static enum hrtimer_restart gate_timer_func(struct hrtimer *timer)
70 struct tcf_gate *gact = container_of(timer, struct tcf_gate,
71 hitimer);
72 struct tcf_gate_params *p = &gact->param;
73 struct tcfg_gate_entry *next;
74 ktime_t close_time, now;
76 spin_lock(&gact->tcf_lock);
78 next = gact->next_entry;
80 /* cycle start, clear pending bit, clear total octets */
81 gact->current_gate_status = next->gate_state ? GATE_ACT_GATE_OPEN : 0;
82 gact->current_entry_octets = 0;
83 gact->current_max_octets = next->maxoctets;
85 gact->current_close_time = ktime_add_ns(gact->current_close_time,
86 next->interval);
88 close_time = gact->current_close_time;
90 if (list_is_last(&next->list, &p->entries))
91 next = list_first_entry(&p->entries,
92 struct tcfg_gate_entry, list);
93 else
94 next = list_next_entry(next, list);
96 now = gate_get_time(gact);
98 if (ktime_after(now, close_time)) {
99 ktime_t cycle, base;
100 u64 n;
102 cycle = p->tcfg_cycletime;
103 base = ns_to_ktime(p->tcfg_basetime);
104 n = div64_u64(ktime_sub_ns(now, base), cycle);
105 close_time = ktime_add_ns(base, (n + 1) * cycle);
108 gact->next_entry = next;
110 hrtimer_set_expires(&gact->hitimer, close_time);
112 spin_unlock(&gact->tcf_lock);
114 return HRTIMER_RESTART;
117 static int tcf_gate_act(struct sk_buff *skb, const struct tc_action *a,
118 struct tcf_result *res)
120 struct tcf_gate *gact = to_gate(a);
122 spin_lock(&gact->tcf_lock);
124 tcf_lastuse_update(&gact->tcf_tm);
125 bstats_update(&gact->tcf_bstats, skb);
127 if (unlikely(gact->current_gate_status & GATE_ACT_PENDING)) {
128 spin_unlock(&gact->tcf_lock);
129 return gact->tcf_action;
132 if (!(gact->current_gate_status & GATE_ACT_GATE_OPEN))
133 goto drop;
135 if (gact->current_max_octets >= 0) {
136 gact->current_entry_octets += qdisc_pkt_len(skb);
137 if (gact->current_entry_octets > gact->current_max_octets) {
138 gact->tcf_qstats.overlimits++;
139 goto drop;
143 spin_unlock(&gact->tcf_lock);
145 return gact->tcf_action;
146 drop:
147 gact->tcf_qstats.drops++;
148 spin_unlock(&gact->tcf_lock);
150 return TC_ACT_SHOT;
153 static const struct nla_policy entry_policy[TCA_GATE_ENTRY_MAX + 1] = {
154 [TCA_GATE_ENTRY_INDEX] = { .type = NLA_U32 },
155 [TCA_GATE_ENTRY_GATE] = { .type = NLA_FLAG },
156 [TCA_GATE_ENTRY_INTERVAL] = { .type = NLA_U32 },
157 [TCA_GATE_ENTRY_IPV] = { .type = NLA_S32 },
158 [TCA_GATE_ENTRY_MAX_OCTETS] = { .type = NLA_S32 },
161 static const struct nla_policy gate_policy[TCA_GATE_MAX + 1] = {
162 [TCA_GATE_PARMS] =
163 NLA_POLICY_EXACT_LEN(sizeof(struct tc_gate)),
164 [TCA_GATE_PRIORITY] = { .type = NLA_S32 },
165 [TCA_GATE_ENTRY_LIST] = { .type = NLA_NESTED },
166 [TCA_GATE_BASE_TIME] = { .type = NLA_U64 },
167 [TCA_GATE_CYCLE_TIME] = { .type = NLA_U64 },
168 [TCA_GATE_CYCLE_TIME_EXT] = { .type = NLA_U64 },
169 [TCA_GATE_FLAGS] = { .type = NLA_U32 },
170 [TCA_GATE_CLOCKID] = { .type = NLA_S32 },
173 static int fill_gate_entry(struct nlattr **tb, struct tcfg_gate_entry *entry,
174 struct netlink_ext_ack *extack)
176 u32 interval = 0;
178 entry->gate_state = nla_get_flag(tb[TCA_GATE_ENTRY_GATE]);
180 if (tb[TCA_GATE_ENTRY_INTERVAL])
181 interval = nla_get_u32(tb[TCA_GATE_ENTRY_INTERVAL]);
183 if (interval == 0) {
184 NL_SET_ERR_MSG(extack, "Invalid interval for schedule entry");
185 return -EINVAL;
188 entry->interval = interval;
190 if (tb[TCA_GATE_ENTRY_IPV])
191 entry->ipv = nla_get_s32(tb[TCA_GATE_ENTRY_IPV]);
192 else
193 entry->ipv = -1;
195 if (tb[TCA_GATE_ENTRY_MAX_OCTETS])
196 entry->maxoctets = nla_get_s32(tb[TCA_GATE_ENTRY_MAX_OCTETS]);
197 else
198 entry->maxoctets = -1;
200 return 0;
203 static int parse_gate_entry(struct nlattr *n, struct tcfg_gate_entry *entry,
204 int index, struct netlink_ext_ack *extack)
206 struct nlattr *tb[TCA_GATE_ENTRY_MAX + 1] = { };
207 int err;
209 err = nla_parse_nested(tb, TCA_GATE_ENTRY_MAX, n, entry_policy, extack);
210 if (err < 0) {
211 NL_SET_ERR_MSG(extack, "Could not parse nested entry");
212 return -EINVAL;
215 entry->index = index;
217 return fill_gate_entry(tb, entry, extack);
220 static void release_entry_list(struct list_head *entries)
222 struct tcfg_gate_entry *entry, *e;
224 list_for_each_entry_safe(entry, e, entries, list) {
225 list_del(&entry->list);
226 kfree(entry);
230 static int parse_gate_list(struct nlattr *list_attr,
231 struct tcf_gate_params *sched,
232 struct netlink_ext_ack *extack)
234 struct tcfg_gate_entry *entry;
235 struct nlattr *n;
236 int err, rem;
237 int i = 0;
239 if (!list_attr)
240 return -EINVAL;
242 nla_for_each_nested(n, list_attr, rem) {
243 if (nla_type(n) != TCA_GATE_ONE_ENTRY) {
244 NL_SET_ERR_MSG(extack, "Attribute isn't type 'entry'");
245 continue;
248 entry = kzalloc(sizeof(*entry), GFP_ATOMIC);
249 if (!entry) {
250 NL_SET_ERR_MSG(extack, "Not enough memory for entry");
251 err = -ENOMEM;
252 goto release_list;
255 err = parse_gate_entry(n, entry, i, extack);
256 if (err < 0) {
257 kfree(entry);
258 goto release_list;
261 list_add_tail(&entry->list, &sched->entries);
262 i++;
265 sched->num_entries = i;
267 return i;
269 release_list:
270 release_entry_list(&sched->entries);
272 return err;
275 static void gate_setup_timer(struct tcf_gate *gact, u64 basetime,
276 enum tk_offsets tko, s32 clockid,
277 bool do_init)
279 if (!do_init) {
280 if (basetime == gact->param.tcfg_basetime &&
281 tko == gact->tk_offset &&
282 clockid == gact->param.tcfg_clockid)
283 return;
285 spin_unlock_bh(&gact->tcf_lock);
286 hrtimer_cancel(&gact->hitimer);
287 spin_lock_bh(&gact->tcf_lock);
289 gact->param.tcfg_basetime = basetime;
290 gact->param.tcfg_clockid = clockid;
291 gact->tk_offset = tko;
292 hrtimer_init(&gact->hitimer, clockid, HRTIMER_MODE_ABS_SOFT);
293 gact->hitimer.function = gate_timer_func;
296 static int tcf_gate_init(struct net *net, struct nlattr *nla,
297 struct nlattr *est, struct tc_action **a,
298 int ovr, int bind, bool rtnl_held,
299 struct tcf_proto *tp, u32 flags,
300 struct netlink_ext_ack *extack)
302 struct tc_action_net *tn = net_generic(net, gate_net_id);
303 enum tk_offsets tk_offset = TK_OFFS_TAI;
304 struct nlattr *tb[TCA_GATE_MAX + 1];
305 struct tcf_chain *goto_ch = NULL;
306 u64 cycletime = 0, basetime = 0;
307 struct tcf_gate_params *p;
308 s32 clockid = CLOCK_TAI;
309 struct tcf_gate *gact;
310 struct tc_gate *parm;
311 int ret = 0, err;
312 u32 gflags = 0;
313 s32 prio = -1;
314 ktime_t start;
315 u32 index;
317 if (!nla)
318 return -EINVAL;
320 err = nla_parse_nested(tb, TCA_GATE_MAX, nla, gate_policy, extack);
321 if (err < 0)
322 return err;
324 if (!tb[TCA_GATE_PARMS])
325 return -EINVAL;
327 if (tb[TCA_GATE_CLOCKID]) {
328 clockid = nla_get_s32(tb[TCA_GATE_CLOCKID]);
329 switch (clockid) {
330 case CLOCK_REALTIME:
331 tk_offset = TK_OFFS_REAL;
332 break;
333 case CLOCK_MONOTONIC:
334 tk_offset = TK_OFFS_MAX;
335 break;
336 case CLOCK_BOOTTIME:
337 tk_offset = TK_OFFS_BOOT;
338 break;
339 case CLOCK_TAI:
340 tk_offset = TK_OFFS_TAI;
341 break;
342 default:
343 NL_SET_ERR_MSG(extack, "Invalid 'clockid'");
344 return -EINVAL;
348 parm = nla_data(tb[TCA_GATE_PARMS]);
349 index = parm->index;
351 err = tcf_idr_check_alloc(tn, &index, a, bind);
352 if (err < 0)
353 return err;
355 if (err && bind)
356 return 0;
358 if (!err) {
359 ret = tcf_idr_create(tn, index, est, a,
360 &act_gate_ops, bind, false, 0);
361 if (ret) {
362 tcf_idr_cleanup(tn, index);
363 return ret;
366 ret = ACT_P_CREATED;
367 } else if (!ovr) {
368 tcf_idr_release(*a, bind);
369 return -EEXIST;
372 if (tb[TCA_GATE_PRIORITY])
373 prio = nla_get_s32(tb[TCA_GATE_PRIORITY]);
375 if (tb[TCA_GATE_BASE_TIME])
376 basetime = nla_get_u64(tb[TCA_GATE_BASE_TIME]);
378 if (tb[TCA_GATE_FLAGS])
379 gflags = nla_get_u32(tb[TCA_GATE_FLAGS]);
381 gact = to_gate(*a);
382 if (ret == ACT_P_CREATED)
383 INIT_LIST_HEAD(&gact->param.entries);
385 err = tcf_action_check_ctrlact(parm->action, tp, &goto_ch, extack);
386 if (err < 0)
387 goto release_idr;
389 spin_lock_bh(&gact->tcf_lock);
390 p = &gact->param;
392 if (tb[TCA_GATE_CYCLE_TIME])
393 cycletime = nla_get_u64(tb[TCA_GATE_CYCLE_TIME]);
395 if (tb[TCA_GATE_ENTRY_LIST]) {
396 err = parse_gate_list(tb[TCA_GATE_ENTRY_LIST], p, extack);
397 if (err < 0)
398 goto chain_put;
401 if (!cycletime) {
402 struct tcfg_gate_entry *entry;
403 ktime_t cycle = 0;
405 list_for_each_entry(entry, &p->entries, list)
406 cycle = ktime_add_ns(cycle, entry->interval);
407 cycletime = cycle;
408 if (!cycletime) {
409 err = -EINVAL;
410 goto chain_put;
413 p->tcfg_cycletime = cycletime;
415 if (tb[TCA_GATE_CYCLE_TIME_EXT])
416 p->tcfg_cycletime_ext =
417 nla_get_u64(tb[TCA_GATE_CYCLE_TIME_EXT]);
419 gate_setup_timer(gact, basetime, tk_offset, clockid,
420 ret == ACT_P_CREATED);
421 p->tcfg_priority = prio;
422 p->tcfg_flags = gflags;
423 gate_get_start_time(gact, &start);
425 gact->current_close_time = start;
426 gact->current_gate_status = GATE_ACT_GATE_OPEN | GATE_ACT_PENDING;
428 gact->next_entry = list_first_entry(&p->entries,
429 struct tcfg_gate_entry, list);
431 goto_ch = tcf_action_set_ctrlact(*a, parm->action, goto_ch);
433 gate_start_timer(gact, start);
435 spin_unlock_bh(&gact->tcf_lock);
437 if (goto_ch)
438 tcf_chain_put_by_act(goto_ch);
440 return ret;
442 chain_put:
443 spin_unlock_bh(&gact->tcf_lock);
445 if (goto_ch)
446 tcf_chain_put_by_act(goto_ch);
447 release_idr:
448 /* action is not inserted in any list: it's safe to init hitimer
449 * without taking tcf_lock.
451 if (ret == ACT_P_CREATED)
452 gate_setup_timer(gact, gact->param.tcfg_basetime,
453 gact->tk_offset, gact->param.tcfg_clockid,
454 true);
455 tcf_idr_release(*a, bind);
456 return err;
459 static void tcf_gate_cleanup(struct tc_action *a)
461 struct tcf_gate *gact = to_gate(a);
462 struct tcf_gate_params *p;
464 p = &gact->param;
465 hrtimer_cancel(&gact->hitimer);
466 release_entry_list(&p->entries);
469 static int dumping_entry(struct sk_buff *skb,
470 struct tcfg_gate_entry *entry)
472 struct nlattr *item;
474 item = nla_nest_start_noflag(skb, TCA_GATE_ONE_ENTRY);
475 if (!item)
476 return -ENOSPC;
478 if (nla_put_u32(skb, TCA_GATE_ENTRY_INDEX, entry->index))
479 goto nla_put_failure;
481 if (entry->gate_state && nla_put_flag(skb, TCA_GATE_ENTRY_GATE))
482 goto nla_put_failure;
484 if (nla_put_u32(skb, TCA_GATE_ENTRY_INTERVAL, entry->interval))
485 goto nla_put_failure;
487 if (nla_put_s32(skb, TCA_GATE_ENTRY_MAX_OCTETS, entry->maxoctets))
488 goto nla_put_failure;
490 if (nla_put_s32(skb, TCA_GATE_ENTRY_IPV, entry->ipv))
491 goto nla_put_failure;
493 return nla_nest_end(skb, item);
495 nla_put_failure:
496 nla_nest_cancel(skb, item);
497 return -1;
500 static int tcf_gate_dump(struct sk_buff *skb, struct tc_action *a,
501 int bind, int ref)
503 unsigned char *b = skb_tail_pointer(skb);
504 struct tcf_gate *gact = to_gate(a);
505 struct tc_gate opt = {
506 .index = gact->tcf_index,
507 .refcnt = refcount_read(&gact->tcf_refcnt) - ref,
508 .bindcnt = atomic_read(&gact->tcf_bindcnt) - bind,
510 struct tcfg_gate_entry *entry;
511 struct tcf_gate_params *p;
512 struct nlattr *entry_list;
513 struct tcf_t t;
515 spin_lock_bh(&gact->tcf_lock);
516 opt.action = gact->tcf_action;
518 p = &gact->param;
520 if (nla_put(skb, TCA_GATE_PARMS, sizeof(opt), &opt))
521 goto nla_put_failure;
523 if (nla_put_u64_64bit(skb, TCA_GATE_BASE_TIME,
524 p->tcfg_basetime, TCA_GATE_PAD))
525 goto nla_put_failure;
527 if (nla_put_u64_64bit(skb, TCA_GATE_CYCLE_TIME,
528 p->tcfg_cycletime, TCA_GATE_PAD))
529 goto nla_put_failure;
531 if (nla_put_u64_64bit(skb, TCA_GATE_CYCLE_TIME_EXT,
532 p->tcfg_cycletime_ext, TCA_GATE_PAD))
533 goto nla_put_failure;
535 if (nla_put_s32(skb, TCA_GATE_CLOCKID, p->tcfg_clockid))
536 goto nla_put_failure;
538 if (nla_put_u32(skb, TCA_GATE_FLAGS, p->tcfg_flags))
539 goto nla_put_failure;
541 if (nla_put_s32(skb, TCA_GATE_PRIORITY, p->tcfg_priority))
542 goto nla_put_failure;
544 entry_list = nla_nest_start_noflag(skb, TCA_GATE_ENTRY_LIST);
545 if (!entry_list)
546 goto nla_put_failure;
548 list_for_each_entry(entry, &p->entries, list) {
549 if (dumping_entry(skb, entry) < 0)
550 goto nla_put_failure;
553 nla_nest_end(skb, entry_list);
555 tcf_tm_dump(&t, &gact->tcf_tm);
556 if (nla_put_64bit(skb, TCA_GATE_TM, sizeof(t), &t, TCA_GATE_PAD))
557 goto nla_put_failure;
558 spin_unlock_bh(&gact->tcf_lock);
560 return skb->len;
562 nla_put_failure:
563 spin_unlock_bh(&gact->tcf_lock);
564 nlmsg_trim(skb, b);
565 return -1;
568 static int tcf_gate_walker(struct net *net, struct sk_buff *skb,
569 struct netlink_callback *cb, int type,
570 const struct tc_action_ops *ops,
571 struct netlink_ext_ack *extack)
573 struct tc_action_net *tn = net_generic(net, gate_net_id);
575 return tcf_generic_walker(tn, skb, cb, type, ops, extack);
578 static void tcf_gate_stats_update(struct tc_action *a, u64 bytes, u64 packets,
579 u64 drops, u64 lastuse, bool hw)
581 struct tcf_gate *gact = to_gate(a);
582 struct tcf_t *tm = &gact->tcf_tm;
584 tcf_action_update_stats(a, bytes, packets, drops, hw);
585 tm->lastuse = max_t(u64, tm->lastuse, lastuse);
588 static int tcf_gate_search(struct net *net, struct tc_action **a, u32 index)
590 struct tc_action_net *tn = net_generic(net, gate_net_id);
592 return tcf_idr_search(tn, a, index);
595 static size_t tcf_gate_get_fill_size(const struct tc_action *act)
597 return nla_total_size(sizeof(struct tc_gate));
600 static struct tc_action_ops act_gate_ops = {
601 .kind = "gate",
602 .id = TCA_ID_GATE,
603 .owner = THIS_MODULE,
604 .act = tcf_gate_act,
605 .dump = tcf_gate_dump,
606 .init = tcf_gate_init,
607 .cleanup = tcf_gate_cleanup,
608 .walk = tcf_gate_walker,
609 .stats_update = tcf_gate_stats_update,
610 .get_fill_size = tcf_gate_get_fill_size,
611 .lookup = tcf_gate_search,
612 .size = sizeof(struct tcf_gate),
615 static __net_init int gate_init_net(struct net *net)
617 struct tc_action_net *tn = net_generic(net, gate_net_id);
619 return tc_action_net_init(net, tn, &act_gate_ops);
622 static void __net_exit gate_exit_net(struct list_head *net_list)
624 tc_action_net_exit(net_list, gate_net_id);
627 static struct pernet_operations gate_net_ops = {
628 .init = gate_init_net,
629 .exit_batch = gate_exit_net,
630 .id = &gate_net_id,
631 .size = sizeof(struct tc_action_net),
634 static int __init gate_init_module(void)
636 return tcf_register_action(&act_gate_ops, &gate_net_ops);
639 static void __exit gate_cleanup_module(void)
641 tcf_unregister_action(&act_gate_ops, &gate_net_ops);
644 module_init(gate_init_module);
645 module_exit(gate_cleanup_module);
646 MODULE_LICENSE("GPL v2");