1
2
3
4#include <net/tc_act/tc_gate.h>
5#include <linux/dsa/8021q.h>
6#include "sja1105_vl.h"
7
8#define SJA1105_SIZE_VL_STATUS 8
9
10
11static int sja1105_insert_gate_entry(struct sja1105_gating_config *gating_cfg,
12 struct sja1105_rule *rule,
13 u8 gate_state, s64 entry_time,
14 struct netlink_ext_ack *extack)
15{
16 struct sja1105_gate_entry *e;
17 int rc;
18
19 e = kzalloc(sizeof(*e), GFP_KERNEL);
20 if (!e)
21 return -ENOMEM;
22
23 e->rule = rule;
24 e->gate_state = gate_state;
25 e->interval = entry_time;
26
27 if (list_empty(&gating_cfg->entries)) {
28 list_add(&e->list, &gating_cfg->entries);
29 } else {
30 struct sja1105_gate_entry *p;
31
32 list_for_each_entry(p, &gating_cfg->entries, list) {
33 if (p->interval == e->interval) {
34 NL_SET_ERR_MSG_MOD(extack,
35 "Gate conflict");
36 rc = -EBUSY;
37 goto err;
38 }
39
40 if (e->interval < p->interval)
41 break;
42 }
43 list_add(&e->list, p->list.prev);
44 }
45
46 gating_cfg->num_entries++;
47
48 return 0;
49err:
50 kfree(e);
51 return rc;
52}
53
54
55
56
57static void
58sja1105_gating_cfg_time_to_interval(struct sja1105_gating_config *gating_cfg,
59 u64 cycle_time)
60{
61 struct sja1105_gate_entry *last_e;
62 struct sja1105_gate_entry *e;
63 struct list_head *prev;
64
65 list_for_each_entry(e, &gating_cfg->entries, list) {
66 struct sja1105_gate_entry *p;
67
68 prev = e->list.prev;
69
70 if (prev == &gating_cfg->entries)
71 continue;
72
73 p = list_entry(prev, struct sja1105_gate_entry, list);
74 p->interval = e->interval - p->interval;
75 }
76 last_e = list_last_entry(&gating_cfg->entries,
77 struct sja1105_gate_entry, list);
78 last_e->interval = cycle_time - last_e->interval;
79}
80
81static void sja1105_free_gating_config(struct sja1105_gating_config *gating_cfg)
82{
83 struct sja1105_gate_entry *e, *n;
84
85 list_for_each_entry_safe(e, n, &gating_cfg->entries, list) {
86 list_del(&e->list);
87 kfree(e);
88 }
89}
90
91static int sja1105_compose_gating_subschedule(struct sja1105_private *priv,
92 struct netlink_ext_ack *extack)
93{
94 struct sja1105_gating_config *gating_cfg = &priv->tas_data.gating_cfg;
95 struct sja1105_rule *rule;
96 s64 max_cycle_time = 0;
97 s64 its_base_time = 0;
98 int i, rc = 0;
99
100 sja1105_free_gating_config(gating_cfg);
101
102 list_for_each_entry(rule, &priv->flow_block.rules, list) {
103 if (rule->type != SJA1105_RULE_VL)
104 continue;
105 if (rule->vl.type != SJA1105_VL_TIME_TRIGGERED)
106 continue;
107
108 if (max_cycle_time < rule->vl.cycle_time) {
109 max_cycle_time = rule->vl.cycle_time;
110 its_base_time = rule->vl.base_time;
111 }
112 }
113
114 if (!max_cycle_time)
115 return 0;
116
117 dev_dbg(priv->ds->dev, "max_cycle_time %lld its_base_time %lld\n",
118 max_cycle_time, its_base_time);
119
120 gating_cfg->base_time = its_base_time;
121 gating_cfg->cycle_time = max_cycle_time;
122 gating_cfg->num_entries = 0;
123
124 list_for_each_entry(rule, &priv->flow_block.rules, list) {
125 s64 time;
126 s64 rbt;
127
128 if (rule->type != SJA1105_RULE_VL)
129 continue;
130 if (rule->vl.type != SJA1105_VL_TIME_TRIGGERED)
131 continue;
132
133
134
135
136
137 rbt = future_base_time(rule->vl.base_time, rule->vl.cycle_time,
138 its_base_time);
139 rbt -= its_base_time;
140
141 time = rbt;
142
143 for (i = 0; i < rule->vl.num_entries; i++) {
144 u8 gate_state = rule->vl.entries[i].gate_state;
145 s64 entry_time = time;
146
147 while (entry_time < max_cycle_time) {
148 rc = sja1105_insert_gate_entry(gating_cfg, rule,
149 gate_state,
150 entry_time,
151 extack);
152 if (rc)
153 goto err;
154
155 entry_time += rule->vl.cycle_time;
156 }
157 time += rule->vl.entries[i].interval;
158 }
159 }
160
161 sja1105_gating_cfg_time_to_interval(gating_cfg, max_cycle_time);
162
163 return 0;
164err:
165 sja1105_free_gating_config(gating_cfg);
166 return rc;
167}
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276static bool sja1105_vl_key_lower(struct sja1105_vl_lookup_entry *a,
277 struct sja1105_vl_lookup_entry *b)
278{
279 if (a->macaddr < b->macaddr)
280 return true;
281 if (a->macaddr > b->macaddr)
282 return false;
283 if (a->vlanid < b->vlanid)
284 return true;
285 if (a->vlanid > b->vlanid)
286 return false;
287 if (a->port < b->port)
288 return true;
289 if (a->port > b->port)
290 return false;
291 if (a->vlanprior < b->vlanprior)
292 return true;
293 if (a->vlanprior > b->vlanprior)
294 return false;
295
296 return false;
297}
298
299static int sja1105_init_virtual_links(struct sja1105_private *priv,
300 struct netlink_ext_ack *extack)
301{
302 struct sja1105_vl_policing_entry *vl_policing;
303 struct sja1105_vl_forwarding_entry *vl_fwd;
304 struct sja1105_vl_lookup_entry *vl_lookup;
305 bool have_critical_virtual_links = false;
306 struct sja1105_table *table;
307 struct sja1105_rule *rule;
308 int num_virtual_links = 0;
309 int max_sharindx = 0;
310 int i, j, k;
311
312
313 list_for_each_entry(rule, &priv->flow_block.rules, list) {
314 if (rule->type != SJA1105_RULE_VL)
315 continue;
316
317 num_virtual_links += hweight_long(rule->port_mask);
318
319 if (rule->vl.type != SJA1105_VL_NONCRITICAL)
320 have_critical_virtual_links = true;
321 if (max_sharindx < rule->vl.sharindx)
322 max_sharindx = rule->vl.sharindx;
323 }
324
325 if (num_virtual_links > SJA1105_MAX_VL_LOOKUP_COUNT) {
326 NL_SET_ERR_MSG_MOD(extack, "Not enough VL entries available");
327 return -ENOSPC;
328 }
329
330 if (max_sharindx + 1 > SJA1105_MAX_VL_LOOKUP_COUNT) {
331 NL_SET_ERR_MSG_MOD(extack, "Policer index out of range");
332 return -ENOSPC;
333 }
334
335 max_sharindx = max_t(int, num_virtual_links, max_sharindx) + 1;
336
337
338 table = &priv->static_config.tables[BLK_IDX_VL_LOOKUP];
339 if (table->entry_count) {
340 kfree(table->entries);
341 table->entry_count = 0;
342 }
343
344
345 table = &priv->static_config.tables[BLK_IDX_VL_POLICING];
346 if (table->entry_count) {
347 kfree(table->entries);
348 table->entry_count = 0;
349 }
350
351
352 table = &priv->static_config.tables[BLK_IDX_VL_FORWARDING];
353 if (table->entry_count) {
354 kfree(table->entries);
355 table->entry_count = 0;
356 }
357
358
359 table = &priv->static_config.tables[BLK_IDX_VL_FORWARDING_PARAMS];
360 if (table->entry_count) {
361 kfree(table->entries);
362 table->entry_count = 0;
363 }
364
365
366 if (!num_virtual_links)
367 return 0;
368
369
370
371
372 table = &priv->static_config.tables[BLK_IDX_VL_LOOKUP];
373 table->entries = kcalloc(num_virtual_links,
374 table->ops->unpacked_entry_size,
375 GFP_KERNEL);
376 if (!table->entries)
377 return -ENOMEM;
378 table->entry_count = num_virtual_links;
379 vl_lookup = table->entries;
380
381 k = 0;
382
383 list_for_each_entry(rule, &priv->flow_block.rules, list) {
384 unsigned long port;
385
386 if (rule->type != SJA1105_RULE_VL)
387 continue;
388
389 for_each_set_bit(port, &rule->port_mask, SJA1105_MAX_NUM_PORTS) {
390 vl_lookup[k].format = SJA1105_VL_FORMAT_PSFP;
391 vl_lookup[k].port = port;
392 vl_lookup[k].macaddr = rule->key.vl.dmac;
393 if (rule->key.type == SJA1105_KEY_VLAN_AWARE_VL) {
394 vl_lookup[k].vlanid = rule->key.vl.vid;
395 vl_lookup[k].vlanprior = rule->key.vl.pcp;
396 } else {
397 u16 vid = dsa_8021q_rx_vid(priv->ds, port);
398
399 vl_lookup[k].vlanid = vid;
400 vl_lookup[k].vlanprior = 0;
401 }
402
403
404
405
406 if (rule->vl.type == SJA1105_VL_NONCRITICAL)
407 vl_lookup[k].destports = rule->vl.destports;
408 else
409 vl_lookup[k].iscritical = true;
410 vl_lookup[k].flow_cookie = rule->cookie;
411 k++;
412 }
413 }
414
415
416
417
418
419
420 for (i = 0; i < num_virtual_links; i++) {
421 struct sja1105_vl_lookup_entry *a = &vl_lookup[i];
422
423 for (j = i + 1; j < num_virtual_links; j++) {
424 struct sja1105_vl_lookup_entry *b = &vl_lookup[j];
425
426 if (sja1105_vl_key_lower(b, a)) {
427 struct sja1105_vl_lookup_entry tmp = *a;
428
429 *a = *b;
430 *b = tmp;
431 }
432 }
433 }
434
435 if (!have_critical_virtual_links)
436 return 0;
437
438
439 table = &priv->static_config.tables[BLK_IDX_VL_POLICING];
440 table->entries = kcalloc(max_sharindx, table->ops->unpacked_entry_size,
441 GFP_KERNEL);
442 if (!table->entries)
443 return -ENOMEM;
444 table->entry_count = max_sharindx;
445 vl_policing = table->entries;
446
447
448 table = &priv->static_config.tables[BLK_IDX_VL_FORWARDING];
449 table->entries = kcalloc(max_sharindx, table->ops->unpacked_entry_size,
450 GFP_KERNEL);
451 if (!table->entries)
452 return -ENOMEM;
453 table->entry_count = max_sharindx;
454 vl_fwd = table->entries;
455
456
457 table = &priv->static_config.tables[BLK_IDX_VL_FORWARDING_PARAMS];
458 table->entries = kcalloc(1, table->ops->unpacked_entry_size,
459 GFP_KERNEL);
460 if (!table->entries)
461 return -ENOMEM;
462 table->entry_count = 1;
463
464 for (i = 0; i < num_virtual_links; i++) {
465 unsigned long cookie = vl_lookup[i].flow_cookie;
466 struct sja1105_rule *rule = sja1105_rule_find(priv, cookie);
467
468 if (rule->vl.type == SJA1105_VL_NONCRITICAL)
469 continue;
470 if (rule->vl.type == SJA1105_VL_TIME_TRIGGERED) {
471 int sharindx = rule->vl.sharindx;
472
473 vl_policing[i].type = 1;
474 vl_policing[i].sharindx = sharindx;
475 vl_policing[i].maxlen = rule->vl.maxlen;
476 vl_policing[sharindx].type = 1;
477
478 vl_fwd[i].type = 1;
479 vl_fwd[sharindx].type = 1;
480 vl_fwd[sharindx].priority = rule->vl.ipv;
481 vl_fwd[sharindx].partition = 0;
482 vl_fwd[sharindx].destports = rule->vl.destports;
483 }
484 }
485
486 sja1105_frame_memory_partitioning(priv);
487
488 return 0;
489}
490
491int sja1105_vl_redirect(struct sja1105_private *priv, int port,
492 struct netlink_ext_ack *extack, unsigned long cookie,
493 struct sja1105_key *key, unsigned long destports,
494 bool append)
495{
496 struct sja1105_rule *rule = sja1105_rule_find(priv, cookie);
497 int rc;
498
499 if (!priv->vlan_aware && key->type != SJA1105_KEY_VLAN_UNAWARE_VL) {
500 NL_SET_ERR_MSG_MOD(extack,
501 "Can only redirect based on DMAC");
502 return -EOPNOTSUPP;
503 } else if (priv->vlan_aware && key->type != SJA1105_KEY_VLAN_AWARE_VL) {
504 NL_SET_ERR_MSG_MOD(extack,
505 "Can only redirect based on {DMAC, VID, PCP}");
506 return -EOPNOTSUPP;
507 }
508
509 if (!rule) {
510 rule = kzalloc(sizeof(*rule), GFP_KERNEL);
511 if (!rule)
512 return -ENOMEM;
513
514 rule->cookie = cookie;
515 rule->type = SJA1105_RULE_VL;
516 rule->key = *key;
517 list_add(&rule->list, &priv->flow_block.rules);
518 }
519
520 rule->port_mask |= BIT(port);
521 if (append)
522 rule->vl.destports |= destports;
523 else
524 rule->vl.destports = destports;
525
526 rc = sja1105_init_virtual_links(priv, extack);
527 if (rc) {
528 rule->port_mask &= ~BIT(port);
529 if (!rule->port_mask) {
530 list_del(&rule->list);
531 kfree(rule);
532 }
533 }
534
535 return rc;
536}
537
538int sja1105_vl_delete(struct sja1105_private *priv, int port,
539 struct sja1105_rule *rule, struct netlink_ext_ack *extack)
540{
541 int rc;
542
543 rule->port_mask &= ~BIT(port);
544 if (!rule->port_mask) {
545 list_del(&rule->list);
546 kfree(rule);
547 }
548
549 rc = sja1105_compose_gating_subschedule(priv, extack);
550 if (rc)
551 return rc;
552
553 rc = sja1105_init_virtual_links(priv, extack);
554 if (rc)
555 return rc;
556
557 rc = sja1105_init_scheduling(priv);
558 if (rc < 0)
559 return rc;
560
561 return sja1105_static_config_reload(priv, SJA1105_VIRTUAL_LINKS);
562}
563
564int sja1105_vl_gate(struct sja1105_private *priv, int port,
565 struct netlink_ext_ack *extack, unsigned long cookie,
566 struct sja1105_key *key, u32 index, s32 prio,
567 u64 base_time, u64 cycle_time, u64 cycle_time_ext,
568 u32 num_entries, struct action_gate_entry *entries)
569{
570 struct sja1105_rule *rule = sja1105_rule_find(priv, cookie);
571 int ipv = -1;
572 int i, rc;
573 s32 rem;
574
575 if (cycle_time_ext) {
576 NL_SET_ERR_MSG_MOD(extack,
577 "Cycle time extension not supported");
578 return -EOPNOTSUPP;
579 }
580
581 div_s64_rem(base_time, sja1105_delta_to_ns(1), &rem);
582 if (rem) {
583 NL_SET_ERR_MSG_MOD(extack,
584 "Base time must be multiple of 200 ns");
585 return -ERANGE;
586 }
587
588 div_s64_rem(cycle_time, sja1105_delta_to_ns(1), &rem);
589 if (rem) {
590 NL_SET_ERR_MSG_MOD(extack,
591 "Cycle time must be multiple of 200 ns");
592 return -ERANGE;
593 }
594
595 if (!priv->vlan_aware && key->type != SJA1105_KEY_VLAN_UNAWARE_VL) {
596 NL_SET_ERR_MSG_MOD(extack,
597 "Can only gate based on DMAC");
598 return -EOPNOTSUPP;
599 } else if (priv->vlan_aware && key->type != SJA1105_KEY_VLAN_AWARE_VL) {
600 NL_SET_ERR_MSG_MOD(extack,
601 "Can only gate based on {DMAC, VID, PCP}");
602 return -EOPNOTSUPP;
603 }
604
605 if (!rule) {
606 rule = kzalloc(sizeof(*rule), GFP_KERNEL);
607 if (!rule)
608 return -ENOMEM;
609
610 list_add(&rule->list, &priv->flow_block.rules);
611 rule->cookie = cookie;
612 rule->type = SJA1105_RULE_VL;
613 rule->key = *key;
614 rule->vl.type = SJA1105_VL_TIME_TRIGGERED;
615 rule->vl.sharindx = index;
616 rule->vl.base_time = base_time;
617 rule->vl.cycle_time = cycle_time;
618 rule->vl.num_entries = num_entries;
619 rule->vl.entries = kcalloc(num_entries,
620 sizeof(struct action_gate_entry),
621 GFP_KERNEL);
622 if (!rule->vl.entries) {
623 rc = -ENOMEM;
624 goto out;
625 }
626
627 for (i = 0; i < num_entries; i++) {
628 div_s64_rem(entries[i].interval,
629 sja1105_delta_to_ns(1), &rem);
630 if (rem) {
631 NL_SET_ERR_MSG_MOD(extack,
632 "Interval must be multiple of 200 ns");
633 rc = -ERANGE;
634 goto out;
635 }
636
637 if (!entries[i].interval) {
638 NL_SET_ERR_MSG_MOD(extack,
639 "Interval cannot be zero");
640 rc = -ERANGE;
641 goto out;
642 }
643
644 if (ns_to_sja1105_delta(entries[i].interval) >
645 SJA1105_TAS_MAX_DELTA) {
646 NL_SET_ERR_MSG_MOD(extack,
647 "Maximum interval is 52 ms");
648 rc = -ERANGE;
649 goto out;
650 }
651
652 if (entries[i].maxoctets != -1) {
653 NL_SET_ERR_MSG_MOD(extack,
654 "Cannot offload IntervalOctetMax");
655 rc = -EOPNOTSUPP;
656 goto out;
657 }
658
659 if (ipv == -1) {
660 ipv = entries[i].ipv;
661 } else if (ipv != entries[i].ipv) {
662 NL_SET_ERR_MSG_MOD(extack,
663 "Only support a single IPV per VL");
664 rc = -EOPNOTSUPP;
665 goto out;
666 }
667
668 rule->vl.entries[i] = entries[i];
669 }
670
671 if (ipv == -1) {
672 if (key->type == SJA1105_KEY_VLAN_AWARE_VL)
673 ipv = key->vl.pcp;
674 else
675 ipv = 0;
676 }
677
678
679 rule->vl.maxlen = VLAN_ETH_FRAME_LEN + ETH_FCS_LEN;
680 rule->vl.ipv = ipv;
681 }
682
683 rule->port_mask |= BIT(port);
684
685 rc = sja1105_compose_gating_subschedule(priv, extack);
686 if (rc)
687 goto out;
688
689 rc = sja1105_init_virtual_links(priv, extack);
690 if (rc)
691 goto out;
692
693 if (sja1105_gating_check_conflicts(priv, -1, extack)) {
694 NL_SET_ERR_MSG_MOD(extack, "Conflict with tc-taprio schedule");
695 rc = -ERANGE;
696 goto out;
697 }
698
699out:
700 if (rc) {
701 rule->port_mask &= ~BIT(port);
702 if (!rule->port_mask) {
703 list_del(&rule->list);
704 kfree(rule->vl.entries);
705 kfree(rule);
706 }
707 }
708
709 return rc;
710}
711
712static int sja1105_find_vlid(struct sja1105_private *priv, int port,
713 struct sja1105_key *key)
714{
715 struct sja1105_vl_lookup_entry *vl_lookup;
716 struct sja1105_table *table;
717 int i;
718
719 if (WARN_ON(key->type != SJA1105_KEY_VLAN_AWARE_VL &&
720 key->type != SJA1105_KEY_VLAN_UNAWARE_VL))
721 return -1;
722
723 table = &priv->static_config.tables[BLK_IDX_VL_LOOKUP];
724 vl_lookup = table->entries;
725
726 for (i = 0; i < table->entry_count; i++) {
727 if (key->type == SJA1105_KEY_VLAN_AWARE_VL) {
728 if (vl_lookup[i].port == port &&
729 vl_lookup[i].macaddr == key->vl.dmac &&
730 vl_lookup[i].vlanid == key->vl.vid &&
731 vl_lookup[i].vlanprior == key->vl.pcp)
732 return i;
733 } else {
734 if (vl_lookup[i].port == port &&
735 vl_lookup[i].macaddr == key->vl.dmac)
736 return i;
737 }
738 }
739
740 return -1;
741}
742
743int sja1105_vl_stats(struct sja1105_private *priv, int port,
744 struct sja1105_rule *rule, struct flow_stats *stats,
745 struct netlink_ext_ack *extack)
746{
747 const struct sja1105_regs *regs = priv->info->regs;
748 u8 buf[SJA1105_SIZE_VL_STATUS] = {0};
749 u64 unreleased;
750 u64 timingerr;
751 u64 lengtherr;
752 int vlid, rc;
753 u64 pkts;
754
755 if (rule->vl.type != SJA1105_VL_TIME_TRIGGERED)
756 return 0;
757
758 vlid = sja1105_find_vlid(priv, port, &rule->key);
759 if (vlid < 0)
760 return 0;
761
762 rc = sja1105_xfer_buf(priv, SPI_READ, regs->vl_status + 2 * vlid, buf,
763 SJA1105_SIZE_VL_STATUS);
764 if (rc) {
765 NL_SET_ERR_MSG_MOD(extack, "SPI access failed");
766 return rc;
767 }
768
769 sja1105_unpack(buf, &timingerr, 31, 16, SJA1105_SIZE_VL_STATUS);
770 sja1105_unpack(buf, &unreleased, 15, 0, SJA1105_SIZE_VL_STATUS);
771 sja1105_unpack(buf, &lengtherr, 47, 32, SJA1105_SIZE_VL_STATUS);
772
773 pkts = timingerr + unreleased + lengtherr;
774
775 flow_stats_update(stats, 0, pkts - rule->vl.stats.pkts, 0,
776 jiffies - rule->vl.stats.lastused,
777 FLOW_ACTION_HW_STATS_IMMEDIATE);
778
779 rule->vl.stats.pkts = pkts;
780 rule->vl.stats.lastused = jiffies;
781
782 return 0;
783}
784