forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2024-05-10 cde9070d9970eef1f7ec2360586c802a16230ad8 
 kernel/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
....@@ -94,7 +94,7 @@
9494 	struct stmmac_tc_entry *entry, *frag = NULL;
9595 	struct tc_u32_sel *sel = cls->knode.sel;
9696 	u32 off, data, mask, real_off, rem;
97
-	u32 prio = cls->common.prio;
97
+	u32 prio = cls->common.prio << 16;
9898 	int ret;
9999 
100100 	/* Only 1 match per entry */
....@@ -228,7 +228,7 @@
228228 	switch (cls->command) {
229229 	case TC_CLSU32_REPLACE_KNODE:
230230 		tc_unfill_entry(priv, cls);
231
-		/* Fall through */
231
+		fallthrough;
232232 	case TC_CLSU32_NEW_KNODE:
233233 		return tc_config_knode(priv, cls);
234234 	case TC_CLSU32_DELETE_KNODE:
....@@ -242,9 +242,27 @@
242242 {
243243 	struct dma_features *dma_cap = &priv->dma_cap;
244244 	unsigned int count;
245
+	int i;
245246 
247
+	if (dma_cap->l3l4fnum) {
248
+		priv->flow_entries_max = dma_cap->l3l4fnum;
249
+		priv->flow_entries = devm_kcalloc(priv->device,
250
+						  dma_cap->l3l4fnum,
251
+						  sizeof(*priv->flow_entries),
252
+						  GFP_KERNEL);
253
+		if (!priv->flow_entries)
254
+			return -ENOMEM;
255
+
256
+		for (i = 0; i < priv->flow_entries_max; i++)
257
+			priv->flow_entries[i].idx = i;
258
+
259
+		dev_info(priv->device, "Enabled Flow TC (entries=%d)\n",
260
+			 priv->flow_entries_max);
261
+	}
262
+
263
+	/* Fail silently as we can still use remaining features, e.g. CBS */
246264 	if (!dma_cap->frpsel)
247
-		return -EINVAL;
265
+		return 0;
248266 
249267 	switch (dma_cap->frpbs) {
250268 	case 0x0:
....@@ -303,8 +321,32 @@
303321 		return -EINVAL;
304322 	if (!priv->dma_cap.av)
305323 		return -EOPNOTSUPP;
306
-	if (priv->speed != SPEED_100 && priv->speed != SPEED_1000)
324
+
325
+	/* Port Transmit Rate and Speed Divider */
326
+	switch (priv->speed) {
327
+	case SPEED_10000:
328
+		ptr = 32;
329
+		speed_div = 10000000;
330
+		break;
331
+	case SPEED_5000:
332
+		ptr = 32;
333
+		speed_div = 5000000;
334
+		break;
335
+	case SPEED_2500:
336
+		ptr = 8;
337
+		speed_div = 2500000;
338
+		break;
339
+	case SPEED_1000:
340
+		ptr = 8;
341
+		speed_div = 1000000;
342
+		break;
343
+	case SPEED_100:
344
+		ptr = 4;
345
+		speed_div = 100000;
346
+		break;
347
+	default:
307348 		return -EOPNOTSUPP;
349
+	}
308350 
309351 	mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
310352 	if (mode_to_use == MTL_QUEUE_DCB && qopt->enable) {
....@@ -321,10 +363,6 @@
321363 
322364 		priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_DCB;
323365 	}
324
-
325
-	/* Port Transmit Rate and Speed Divider */
326
-	ptr = (priv->speed == SPEED_100) ? 4 : 8;
327
-	speed_div = (priv->speed == SPEED_100) ? 100000 : 1000000;
328366 
329367 	/* Final adjustments for HW */
330368 	value = div_s64(qopt->idleslope * 1024ll * ptr, speed_div);
....@@ -354,8 +392,430 @@
354392 	return 0;
355393 }
356394 
395
+static int tc_parse_flow_actions(struct stmmac_priv *priv,
396
+				 struct flow_action *action,
397
+				 struct stmmac_flow_entry *entry,
398
+				 struct netlink_ext_ack *extack)
399
+{
400
+	struct flow_action_entry *act;
401
+	int i;
402
+
403
+	if (!flow_action_has_entries(action))
404
+		return -EINVAL;
405
+
406
+	if (!flow_action_basic_hw_stats_check(action, extack))
407
+		return -EOPNOTSUPP;
408
+
409
+	flow_action_for_each(i, act, action) {
410
+		switch (act->id) {
411
+		case FLOW_ACTION_DROP:
412
+			entry->action |= STMMAC_FLOW_ACTION_DROP;
413
+			return 0;
414
+		default:
415
+			break;
416
+		}
417
+	}
418
+
419
+	/* Nothing to do, maybe inverse filter ? */
420
+	return 0;
421
+}
422
+
423
+static int tc_add_basic_flow(struct stmmac_priv *priv,
424
+			     struct flow_cls_offload *cls,
425
+			     struct stmmac_flow_entry *entry)
426
+{
427
+	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
428
+	struct flow_dissector *dissector = rule->match.dissector;
429
+	struct flow_match_basic match;
430
+
431
+	/* Nothing to do here */
432
+	if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_BASIC))
433
+		return -EINVAL;
434
+
435
+	flow_rule_match_basic(rule, &match);
436
+	entry->ip_proto = match.key->ip_proto;
437
+	return 0;
438
+}
439
+
440
+static int tc_add_ip4_flow(struct stmmac_priv *priv,
441
+			   struct flow_cls_offload *cls,
442
+			   struct stmmac_flow_entry *entry)
443
+{
444
+	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
445
+	struct flow_dissector *dissector = rule->match.dissector;
446
+	bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
447
+	struct flow_match_ipv4_addrs match;
448
+	u32 hw_match;
449
+	int ret;
450
+
451
+	/* Nothing to do here */
452
+	if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_IPV4_ADDRS))
453
+		return -EINVAL;
454
+
455
+	flow_rule_match_ipv4_addrs(rule, &match);
456
+	hw_match = ntohl(match.key->src) & ntohl(match.mask->src);
457
+	if (hw_match) {
458
+		ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
459
+					      false, true, inv, hw_match);
460
+		if (ret)
461
+			return ret;
462
+	}
463
+
464
+	hw_match = ntohl(match.key->dst) & ntohl(match.mask->dst);
465
+	if (hw_match) {
466
+		ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
467
+					      false, false, inv, hw_match);
468
+		if (ret)
469
+			return ret;
470
+	}
471
+
472
+	return 0;
473
+}
474
+
475
+static int tc_add_ports_flow(struct stmmac_priv *priv,
476
+			     struct flow_cls_offload *cls,
477
+			     struct stmmac_flow_entry *entry)
478
+{
479
+	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
480
+	struct flow_dissector *dissector = rule->match.dissector;
481
+	bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
482
+	struct flow_match_ports match;
483
+	u32 hw_match;
484
+	bool is_udp;
485
+	int ret;
486
+
487
+	/* Nothing to do here */
488
+	if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_PORTS))
489
+		return -EINVAL;
490
+
491
+	switch (entry->ip_proto) {
492
+	case IPPROTO_TCP:
493
+		is_udp = false;
494
+		break;
495
+	case IPPROTO_UDP:
496
+		is_udp = true;
497
+		break;
498
+	default:
499
+		return -EINVAL;
500
+	}
501
+
502
+	flow_rule_match_ports(rule, &match);
503
+
504
+	hw_match = ntohs(match.key->src) & ntohs(match.mask->src);
505
+	if (hw_match) {
506
+		ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
507
+					      is_udp, true, inv, hw_match);
508
+		if (ret)
509
+			return ret;
510
+	}
511
+
512
+	hw_match = ntohs(match.key->dst) & ntohs(match.mask->dst);
513
+	if (hw_match) {
514
+		ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
515
+					      is_udp, false, inv, hw_match);
516
+		if (ret)
517
+			return ret;
518
+	}
519
+
520
+	entry->is_l4 = true;
521
+	return 0;
522
+}
523
+
524
+static struct stmmac_flow_entry *tc_find_flow(struct stmmac_priv *priv,
525
+					      struct flow_cls_offload *cls,
526
+					      bool get_free)
527
+{
528
+	int i;
529
+
530
+	for (i = 0; i < priv->flow_entries_max; i++) {
531
+		struct stmmac_flow_entry *entry = &priv->flow_entries[i];
532
+
533
+		if (entry->cookie == cls->cookie)
534
+			return entry;
535
+		if (get_free && (entry->in_use == false))
536
+			return entry;
537
+	}
538
+
539
+	return NULL;
540
+}
541
+
542
+static struct {
543
+	int (*fn)(struct stmmac_priv *priv, struct flow_cls_offload *cls,
544
+		  struct stmmac_flow_entry *entry);
545
+} tc_flow_parsers[] = {
546
+	{ .fn = tc_add_basic_flow },
547
+	{ .fn = tc_add_ip4_flow },
548
+	{ .fn = tc_add_ports_flow },
549
+};
550
+
551
+static int tc_add_flow(struct stmmac_priv *priv,
552
+		       struct flow_cls_offload *cls)
553
+{
554
+	struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
555
+	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
556
+	int i, ret;
557
+
558
+	if (!entry) {
559
+		entry = tc_find_flow(priv, cls, true);
560
+		if (!entry)
561
+			return -ENOENT;
562
+	}
563
+
564
+	ret = tc_parse_flow_actions(priv, &rule->action, entry,
565
+				    cls->common.extack);
566
+	if (ret)
567
+		return ret;
568
+
569
+	for (i = 0; i < ARRAY_SIZE(tc_flow_parsers); i++) {
570
+		ret = tc_flow_parsers[i].fn(priv, cls, entry);
571
+		if (!ret) {
572
+			entry->in_use = true;
573
+			continue;
574
+		}
575
+	}
576
+
577
+	if (!entry->in_use)
578
+		return -EINVAL;
579
+
580
+	entry->cookie = cls->cookie;
581
+	return 0;
582
+}
583
+
584
+static int tc_del_flow(struct stmmac_priv *priv,
585
+		       struct flow_cls_offload *cls)
586
+{
587
+	struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
588
+	int ret;
589
+
590
+	if (!entry || !entry->in_use)
591
+		return -ENOENT;
592
+
593
+	if (entry->is_l4) {
594
+		ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, false,
595
+					      false, false, false, 0);
596
+	} else {
597
+		ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, false,
598
+					      false, false, false, 0);
599
+	}
600
+
601
+	entry->in_use = false;
602
+	entry->cookie = 0;
603
+	entry->is_l4 = false;
604
+	return ret;
605
+}
606
+
607
+static int tc_setup_cls(struct stmmac_priv *priv,
608
+			struct flow_cls_offload *cls)
609
+{
610
+	int ret = 0;
611
+
612
+	/* When RSS is enabled, the filtering will be bypassed */
613
+	if (priv->rss.enable)
614
+		return -EBUSY;
615
+
616
+	switch (cls->command) {
617
+	case FLOW_CLS_REPLACE:
618
+		ret = tc_add_flow(priv, cls);
619
+		break;
620
+	case FLOW_CLS_DESTROY:
621
+		ret = tc_del_flow(priv, cls);
622
+		break;
623
+	default:
624
+		return -EOPNOTSUPP;
625
+	}
626
+
627
+	return ret;
628
+}
629
+
630
+static int tc_setup_taprio(struct stmmac_priv *priv,
631
+			   struct tc_taprio_qopt_offload *qopt)
632
+{
633
+	u32 size, wid = priv->dma_cap.estwid, dep = priv->dma_cap.estdep;
634
+	struct plat_stmmacenet_data *plat = priv->plat;
635
+	struct timespec64 time, current_time;
636
+	ktime_t current_time_ns;
637
+	bool fpe = false;
638
+	int i, ret = 0;
639
+	u64 ctr;
640
+
641
+	if (!priv->dma_cap.estsel)
642
+		return -EOPNOTSUPP;
643
+
644
+	switch (wid) {
645
+	case 0x1:
646
+		wid = 16;
647
+		break;
648
+	case 0x2:
649
+		wid = 20;
650
+		break;
651
+	case 0x3:
652
+		wid = 24;
653
+		break;
654
+	default:
655
+		return -EOPNOTSUPP;
656
+	}
657
+
658
+	switch (dep) {
659
+	case 0x1:
660
+		dep = 64;
661
+		break;
662
+	case 0x2:
663
+		dep = 128;
664
+		break;
665
+	case 0x3:
666
+		dep = 256;
667
+		break;
668
+	case 0x4:
669
+		dep = 512;
670
+		break;
671
+	case 0x5:
672
+		dep = 1024;
673
+		break;
674
+	default:
675
+		return -EOPNOTSUPP;
676
+	}
677
+
678
+	if (!qopt->enable)
679
+		goto disable;
680
+	if (qopt->num_entries >= dep)
681
+		return -EINVAL;
682
+	if (!qopt->cycle_time)
683
+		return -ERANGE;
684
+
685
+	if (!plat->est) {
686
+		plat->est = devm_kzalloc(priv->device, sizeof(*plat->est),
687
+					 GFP_KERNEL);
688
+		if (!plat->est)
689
+			return -ENOMEM;
690
+
691
+		mutex_init(&priv->plat->est->lock);
692
+	} else {
693
+		memset(plat->est, 0, sizeof(*plat->est));
694
+	}
695
+
696
+	size = qopt->num_entries;
697
+
698
+	mutex_lock(&priv->plat->est->lock);
699
+	priv->plat->est->gcl_size = size;
700
+	priv->plat->est->enable = qopt->enable;
701
+	mutex_unlock(&priv->plat->est->lock);
702
+
703
+	for (i = 0; i < size; i++) {
704
+		s64 delta_ns = qopt->entries[i].interval;
705
+		u32 gates = qopt->entries[i].gate_mask;
706
+
707
+		if (delta_ns > GENMASK(wid, 0))
708
+			return -ERANGE;
709
+		if (gates > GENMASK(31 - wid, 0))
710
+			return -ERANGE;
711
+
712
+		switch (qopt->entries[i].command) {
713
+		case TC_TAPRIO_CMD_SET_GATES:
714
+			if (fpe)
715
+				return -EINVAL;
716
+			break;
717
+		case TC_TAPRIO_CMD_SET_AND_HOLD:
718
+			gates |= BIT(0);
719
+			fpe = true;
720
+			break;
721
+		case TC_TAPRIO_CMD_SET_AND_RELEASE:
722
+			gates &= ~BIT(0);
723
+			fpe = true;
724
+			break;
725
+		default:
726
+			return -EOPNOTSUPP;
727
+		}
728
+
729
+		priv->plat->est->gcl[i] = delta_ns | (gates << wid);
730
+	}
731
+
732
+	mutex_lock(&priv->plat->est->lock);
733
+	/* Adjust for real system time */
734
+	priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, &current_time);
735
+	current_time_ns = timespec64_to_ktime(current_time);
736
+	if (ktime_after(qopt->base_time, current_time_ns)) {
737
+		time = ktime_to_timespec64(qopt->base_time);
738
+	} else {
739
+		ktime_t base_time;
740
+		s64 n;
741
+
742
+		n = div64_s64(ktime_sub_ns(current_time_ns, qopt->base_time),
743
+			      qopt->cycle_time);
744
+		base_time = ktime_add_ns(qopt->base_time,
745
+					 (n + 1) * qopt->cycle_time);
746
+
747
+		time = ktime_to_timespec64(base_time);
748
+	}
749
+
750
+	priv->plat->est->btr[0] = (u32)time.tv_nsec;
751
+	priv->plat->est->btr[1] = (u32)time.tv_sec;
752
+
753
+	ctr = qopt->cycle_time;
754
+	priv->plat->est->ctr[0] = do_div(ctr, NSEC_PER_SEC);
755
+	priv->plat->est->ctr[1] = (u32)ctr;
756
+
757
+	if (fpe && !priv->dma_cap.fpesel) {
758
+		mutex_unlock(&priv->plat->est->lock);
759
+		return -EOPNOTSUPP;
760
+	}
761
+
762
+	ret = stmmac_fpe_configure(priv, priv->ioaddr,
763
+				   priv->plat->tx_queues_to_use,
764
+				   priv->plat->rx_queues_to_use, fpe);
765
+	if (ret && fpe) {
766
+		mutex_unlock(&priv->plat->est->lock);
767
+		netdev_err(priv->dev, "failed to enable Frame Preemption\n");
768
+		return ret;
769
+	}
770
+
771
+	ret = stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
772
+				   priv->plat->clk_ptp_rate);
773
+	mutex_unlock(&priv->plat->est->lock);
774
+	if (ret) {
775
+		netdev_err(priv->dev, "failed to configure EST\n");
776
+		goto disable;
777
+	}
778
+
779
+	netdev_info(priv->dev, "configured EST\n");
780
+	return 0;
781
+
782
+disable:
783
+	if (priv->plat->est) {
784
+		mutex_lock(&priv->plat->est->lock);
785
+		priv->plat->est->enable = false;
786
+		stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
787
+				     priv->plat->clk_ptp_rate);
788
+		mutex_unlock(&priv->plat->est->lock);
789
+	}
790
+
791
+	return ret;
792
+}
793
+
794
+static int tc_setup_etf(struct stmmac_priv *priv,
795
+			struct tc_etf_qopt_offload *qopt)
796
+{
797
+	if (!priv->dma_cap.tbssel)
798
+		return -EOPNOTSUPP;
799
+	if (qopt->queue >= priv->plat->tx_queues_to_use)
800
+		return -EINVAL;
801
+	if (!(priv->tx_queue[qopt->queue].tbs & STMMAC_TBS_AVAIL))
802
+		return -EINVAL;
803
+
804
+	if (qopt->enable)
805
+		priv->tx_queue[qopt->queue].tbs |= STMMAC_TBS_EN;
806
+	else
807
+		priv->tx_queue[qopt->queue].tbs &= ~STMMAC_TBS_EN;
808
+
809
+	netdev_info(priv->dev, "%s ETF for Queue %d\n",
810
+		    qopt->enable ? "enabled" : "disabled", qopt->queue);
811
+	return 0;
812
+}
813
+
357814 const struct stmmac_tc_ops dwmac510_tc_ops = {
358815 	.init = tc_init,
359816 	.setup_cls_u32 = tc_setup_cls_u32,
360817 	.setup_cbs = tc_setup_cbs,
818
+	.setup_cls = tc_setup_cls,
819
+	.setup_taprio = tc_setup_taprio,
820
+	.setup_etf = tc_setup_etf,
361821 };