forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2024-01-05 071106ecf68c401173c58808b1cf5f68cc50d390 
 kernel/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
....@@ -1,3 +1,4 @@
1
+// SPDX-License-Identifier: GPL-2.0-only
12 /*
23  * This is the driver for the GMAC on-chip Ethernet controller for ST SoCs.
34  * DWC Ether MAC version 4.00  has been used for developing this code.
....@@ -5,10 +6,6 @@
56  * This only implements the mac core functions for this chip.
67  *
78  * Copyright (C) 2015  STMicroelectronics Ltd
8
- *
9
- * This program is free software; you can redistribute it and/or modify it
10
- * under the terms and conditions of the GNU General Public License,
11
- * version 2, as published by the Free Software Foundation.
129  *
1310  * Author: Alexandre Torgue <alexandre.torgue@st.com>
1411  */
....@@ -28,14 +25,8 @@
2825 {
2926 	void __iomem *ioaddr = hw->pcsr;
3027 	u32 value = readl(ioaddr + GMAC_CONFIG);
31
-	int mtu = dev->mtu;
3228 
3329 	value |= GMAC_CORE_INIT;
34
-
35
-	if (mtu > 1500)
36
-		value |= GMAC_CONFIG_2K;
37
-	if (mtu > 2000)
38
-		value |= GMAC_CONFIG_JE;
3930 
4031 	if (hw->ps) {
4132 		value |= GMAC_CONFIG_TE;
....@@ -196,6 +187,8 @@
196187 	default:
197188 		break;
198189 	}
190
+
191
+	writel(value, ioaddr + MTL_OPERATION_MODE);
199192 }
200193 
201194 static void dwmac4_set_mtl_tx_queue_weight(struct mac_device_info *hw,
....@@ -222,6 +215,9 @@
222215 	if (queue == 0 || queue == 4) {
223216 		value &= ~MTL_RXQ_DMA_Q04MDMACH_MASK;
224217 		value |= MTL_RXQ_DMA_Q04MDMACH(chan);
218
+	} else if (queue > 4) {
219
+		value &= ~MTL_RXQ_DMA_QXMDMACH_MASK(queue - 4);
220
+		value |= MTL_RXQ_DMA_QXMDMACH(chan, queue - 4);
225221 	} else {
226222 		value &= ~MTL_RXQ_DMA_QXMDMACH_MASK(queue);
227223 		value |= MTL_RXQ_DMA_QXMDMACH(chan, queue);
....@@ -401,46 +397,230 @@
401397 	writel(value, ioaddr + GMAC4_LPI_TIMER_CTRL);
402398 }
403399 
400
+static void dwmac4_write_single_vlan(struct net_device *dev, u16 vid)
401
+{
402
+	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
403
+	u32 val;
404
+
405
+	val = readl(ioaddr + GMAC_VLAN_TAG);
406
+	val &= ~GMAC_VLAN_TAG_VID;
407
+	val |= GMAC_VLAN_TAG_ETV | vid;
408
+
409
+	writel(val, ioaddr + GMAC_VLAN_TAG);
410
+}
411
+
412
+static int dwmac4_write_vlan_filter(struct net_device *dev,
413
+				    struct mac_device_info *hw,
414
+				    u8 index, u32 data)
415
+{
416
+	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
417
+	int i, timeout = 10;
418
+	u32 val;
419
+
420
+	if (index >= hw->num_vlan)
421
+		return -EINVAL;
422
+
423
+	writel(data, ioaddr + GMAC_VLAN_TAG_DATA);
424
+
425
+	val = readl(ioaddr + GMAC_VLAN_TAG);
426
+	val &= ~(GMAC_VLAN_TAG_CTRL_OFS_MASK |
427
+		GMAC_VLAN_TAG_CTRL_CT |
428
+		GMAC_VLAN_TAG_CTRL_OB);
429
+	val |= (index << GMAC_VLAN_TAG_CTRL_OFS_SHIFT) | GMAC_VLAN_TAG_CTRL_OB;
430
+
431
+	writel(val, ioaddr + GMAC_VLAN_TAG);
432
+
433
+	for (i = 0; i < timeout; i++) {
434
+		val = readl(ioaddr + GMAC_VLAN_TAG);
435
+		if (!(val & GMAC_VLAN_TAG_CTRL_OB))
436
+			return 0;
437
+		udelay(1);
438
+	}
439
+
440
+	netdev_err(dev, "Timeout accessing MAC_VLAN_Tag_Filter\n");
441
+
442
+	return -EBUSY;
443
+}
444
+
445
+static int dwmac4_add_hw_vlan_rx_fltr(struct net_device *dev,
446
+				      struct mac_device_info *hw,
447
+				      __be16 proto, u16 vid)
448
+{
449
+	int index = -1;
450
+	u32 val = 0;
451
+	int i, ret;
452
+
453
+	if (vid > 4095)
454
+		return -EINVAL;
455
+
456
+	/* Single Rx VLAN Filter */
457
+	if (hw->num_vlan == 1) {
458
+		/* For single VLAN filter, VID 0 means VLAN promiscuous */
459
+		if (vid == 0) {
460
+			netdev_warn(dev, "Adding VLAN ID 0 is not supported\n");
461
+			return -EPERM;
462
+		}
463
+
464
+		if (hw->vlan_filter[0] & GMAC_VLAN_TAG_VID) {
465
+			netdev_err(dev, "Only single VLAN ID supported\n");
466
+			return -EPERM;
467
+		}
468
+
469
+		hw->vlan_filter[0] = vid;
470
+		dwmac4_write_single_vlan(dev, vid);
471
+
472
+		return 0;
473
+	}
474
+
475
+	/* Extended Rx VLAN Filter Enable */
476
+	val |= GMAC_VLAN_TAG_DATA_ETV | GMAC_VLAN_TAG_DATA_VEN | vid;
477
+
478
+	for (i = 0; i < hw->num_vlan; i++) {
479
+		if (hw->vlan_filter[i] == val)
480
+			return 0;
481
+		else if (!(hw->vlan_filter[i] & GMAC_VLAN_TAG_DATA_VEN))
482
+			index = i;
483
+	}
484
+
485
+	if (index == -1) {
486
+		netdev_err(dev, "MAC_VLAN_Tag_Filter full (size: %0u)\n",
487
+			   hw->num_vlan);
488
+		return -EPERM;
489
+	}
490
+
491
+	ret = dwmac4_write_vlan_filter(dev, hw, index, val);
492
+
493
+	if (!ret)
494
+		hw->vlan_filter[index] = val;
495
+
496
+	return ret;
497
+}
498
+
499
+static int dwmac4_del_hw_vlan_rx_fltr(struct net_device *dev,
500
+				      struct mac_device_info *hw,
501
+				      __be16 proto, u16 vid)
502
+{
503
+	int i, ret = 0;
504
+
505
+	/* Single Rx VLAN Filter */
506
+	if (hw->num_vlan == 1) {
507
+		if ((hw->vlan_filter[0] & GMAC_VLAN_TAG_VID) == vid) {
508
+			hw->vlan_filter[0] = 0;
509
+			dwmac4_write_single_vlan(dev, 0);
510
+		}
511
+		return 0;
512
+	}
513
+
514
+	/* Extended Rx VLAN Filter Enable */
515
+	for (i = 0; i < hw->num_vlan; i++) {
516
+		if ((hw->vlan_filter[i] & GMAC_VLAN_TAG_DATA_VID) == vid) {
517
+			ret = dwmac4_write_vlan_filter(dev, hw, i, 0);
518
+
519
+			if (!ret)
520
+				hw->vlan_filter[i] = 0;
521
+			else
522
+				return ret;
523
+		}
524
+	}
525
+
526
+	return ret;
527
+}
528
+
529
+static void dwmac4_restore_hw_vlan_rx_fltr(struct net_device *dev,
530
+					   struct mac_device_info *hw)
531
+{
532
+	void __iomem *ioaddr = hw->pcsr;
533
+	u32 value;
534
+	u32 hash;
535
+	u32 val;
536
+	int i;
537
+
538
+	/* Single Rx VLAN Filter */
539
+	if (hw->num_vlan == 1) {
540
+		dwmac4_write_single_vlan(dev, hw->vlan_filter[0]);
541
+		return;
542
+	}
543
+
544
+	/* Extended Rx VLAN Filter Enable */
545
+	for (i = 0; i < hw->num_vlan; i++) {
546
+		if (hw->vlan_filter[i] & GMAC_VLAN_TAG_DATA_VEN) {
547
+			val = hw->vlan_filter[i];
548
+			dwmac4_write_vlan_filter(dev, hw, i, val);
549
+		}
550
+	}
551
+
552
+	hash = readl(ioaddr + GMAC_VLAN_HASH_TABLE);
553
+	if (hash & GMAC_VLAN_VLHT) {
554
+		value = readl(ioaddr + GMAC_VLAN_TAG);
555
+		value |= GMAC_VLAN_VTHM;
556
+		writel(value, ioaddr + GMAC_VLAN_TAG);
557
+	}
558
+}
559
+
404560 static void dwmac4_set_filter(struct mac_device_info *hw,
405561 			      struct net_device *dev)
406562 {
407563 	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
408
-	unsigned int value = 0;
564
+	int numhashregs = (hw->multicast_filter_bins >> 5);
565
+	int mcbitslog2 = hw->mcast_bits_log2;
566
+	unsigned int value;
567
+	u32 mc_filter[8];
568
+	int i;
409569 
570
+	memset(mc_filter, 0, sizeof(mc_filter));
571
+
572
+	value = readl(ioaddr + GMAC_PACKET_FILTER);
573
+	value &= ~GMAC_PACKET_FILTER_HMC;
574
+	value &= ~GMAC_PACKET_FILTER_HPF;
575
+	value &= ~GMAC_PACKET_FILTER_PCF;
576
+	value &= ~GMAC_PACKET_FILTER_PM;
577
+	value &= ~GMAC_PACKET_FILTER_PR;
578
+	value &= ~GMAC_PACKET_FILTER_RA;
410579 	if (dev->flags & IFF_PROMISC) {
411
-		value = GMAC_PACKET_FILTER_PR;
580
+		/* VLAN Tag Filter Fail Packets Queuing */
581
+		if (hw->vlan_fail_q_en) {
582
+			value = readl(ioaddr + GMAC_RXQ_CTRL4);
583
+			value &= ~GMAC_RXQCTRL_VFFQ_MASK;
584
+			value |= GMAC_RXQCTRL_VFFQE |
585
+				 (hw->vlan_fail_q << GMAC_RXQCTRL_VFFQ_SHIFT);
586
+			writel(value, ioaddr + GMAC_RXQ_CTRL4);
587
+			value = GMAC_PACKET_FILTER_PR | GMAC_PACKET_FILTER_RA;
588
+		} else {
589
+			value = GMAC_PACKET_FILTER_PR | GMAC_PACKET_FILTER_PCF;
590
+		}
591
+
412592 	} else if ((dev->flags & IFF_ALLMULTI) ||
413
-			(netdev_mc_count(dev) > HASH_TABLE_SIZE)) {
593
+		   (netdev_mc_count(dev) > hw->multicast_filter_bins)) {
414594 		/* Pass all multi */
415
-		value = GMAC_PACKET_FILTER_PM;
416
-		/* Set the 64 bits of the HASH tab. To be updated if taller
417
-		 * hash table is used
418
-		 */
419
-		writel(0xffffffff, ioaddr + GMAC_HASH_TAB_0_31);
420
-		writel(0xffffffff, ioaddr + GMAC_HASH_TAB_32_63);
421
-	} else if (!netdev_mc_empty(dev)) {
422
-		u32 mc_filter[2];
595
+		value |= GMAC_PACKET_FILTER_PM;
596
+		/* Set all the bits of the HASH tab */
597
+		memset(mc_filter, 0xff, sizeof(mc_filter));
598
+	} else if (!netdev_mc_empty(dev) && (dev->flags & IFF_MULTICAST)) {
423599 		struct netdev_hw_addr *ha;
424600 
425601 		/* Hash filter for multicast */
426
-		value = GMAC_PACKET_FILTER_HMC;
602
+		value |= GMAC_PACKET_FILTER_HMC;
427603 
428
-		memset(mc_filter, 0, sizeof(mc_filter));
429604 		netdev_for_each_mc_addr(ha, dev) {
430
-			/* The upper 6 bits of the calculated CRC are used to
431
-			 * index the content of the Hash Table Reg 0 and 1.
605
+			/* The upper n bits of the calculated CRC are used to
606
+			 * index the contents of the hash table. The number of
607
+			 * bits used depends on the hardware configuration
608
+			 * selected at core configuration time.
432609 			 */
433
-			int bit_nr =
434
-				(bitrev32(~crc32_le(~0, ha->addr, 6)) >> 26);
435
-			/* The most significant bit determines the register
436
-			 * to use while the other 5 bits determines the bit
437
-			 * within the selected register
610
+			u32 bit_nr = bitrev32(~crc32_le(~0, ha->addr,
611
+					ETH_ALEN)) >> (32 - mcbitslog2);
612
+			/* The most significant bit determines the register to
613
+			 * use (H/L) while the other 5 bits determine the bit
614
+			 * within the register.
438615 			 */
439
-			mc_filter[bit_nr >> 5] |= (1 << (bit_nr & 0x1F));
616
+			mc_filter[bit_nr >> 5] |= (1 << (bit_nr & 0x1f));
440617 		}
441
-		writel(mc_filter[0], ioaddr + GMAC_HASH_TAB_0_31);
442
-		writel(mc_filter[1], ioaddr + GMAC_HASH_TAB_32_63);
443618 	}
619
+
620
+	for (i = 0; i < numhashregs; i++)
621
+		writel(mc_filter[i], ioaddr + GMAC_HASH_TAB(i));
622
+
623
+	value |= GMAC_PACKET_FILTER_HPF;
444624 
445625 	/* Handle multiple unicast addresses */
446626 	if (netdev_uc_count(dev) > hw->unicast_filter_entries) {
....@@ -457,12 +637,18 @@
457637 			reg++;
458638 		}
459639 
460
-		while (reg <= GMAC_MAX_PERFECT_ADDRESSES) {
640
+		while (reg < GMAC_MAX_PERFECT_ADDRESSES) {
461641 			writel(0, ioaddr + GMAC_ADDR_HIGH(reg));
462642 			writel(0, ioaddr + GMAC_ADDR_LOW(reg));
463643 			reg++;
464644 		}
465645 	}
646
+
647
+	/* VLAN filtering */
648
+	if (dev->flags & IFF_PROMISC && !hw->vlan_fail_q_en)
649
+		value &= ~GMAC_PACKET_FILTER_VTFE;
650
+	else if (dev->features & NETIF_F_HW_VLAN_CTAG_FILTER)
651
+		value |= GMAC_PACKET_FILTER_VTFE;
466652 
467653 	writel(value, ioaddr + GMAC_PACKET_FILTER);
468654 }
....@@ -479,6 +665,8 @@
479665 	if (fc & FLOW_RX) {
480666 		pr_debug("\tReceive Flow-Control ON\n");
481667 		flow |= GMAC_RX_FLOW_CTRL_RFE;
668
+	} else {
669
+		pr_debug("\tReceive Flow-Control OFF\n");
482670 	}
483671 	writel(flow, ioaddr + GMAC_RX_FLOW_CTRL);
484672 
....@@ -715,6 +903,196 @@
715903 		x->mac_gmii_rx_proto_engine++;
716904 }
717905 
906
+static void dwmac4_set_mac_loopback(void __iomem *ioaddr, bool enable)
907
+{
908
+	u32 value = readl(ioaddr + GMAC_CONFIG);
909
+
910
+	if (enable)
911
+		value |= GMAC_CONFIG_LM;
912
+	else
913
+		value &= ~GMAC_CONFIG_LM;
914
+
915
+	writel(value, ioaddr + GMAC_CONFIG);
916
+}
917
+
918
+static void dwmac4_update_vlan_hash(struct mac_device_info *hw, u32 hash,
919
+				    __le16 perfect_match, bool is_double)
920
+{
921
+	void __iomem *ioaddr = hw->pcsr;
922
+	u32 value;
923
+
924
+	writel(hash, ioaddr + GMAC_VLAN_HASH_TABLE);
925
+
926
+	value = readl(ioaddr + GMAC_VLAN_TAG);
927
+
928
+	if (hash) {
929
+		value |= GMAC_VLAN_VTHM | GMAC_VLAN_ETV;
930
+		if (is_double) {
931
+			value |= GMAC_VLAN_EDVLP;
932
+			value |= GMAC_VLAN_ESVL;
933
+			value |= GMAC_VLAN_DOVLTC;
934
+		}
935
+
936
+		writel(value, ioaddr + GMAC_VLAN_TAG);
937
+	} else if (perfect_match) {
938
+		u32 value = GMAC_VLAN_ETV;
939
+
940
+		if (is_double) {
941
+			value |= GMAC_VLAN_EDVLP;
942
+			value |= GMAC_VLAN_ESVL;
943
+			value |= GMAC_VLAN_DOVLTC;
944
+		}
945
+
946
+		writel(value | perfect_match, ioaddr + GMAC_VLAN_TAG);
947
+	} else {
948
+		value &= ~(GMAC_VLAN_VTHM | GMAC_VLAN_ETV);
949
+		value &= ~(GMAC_VLAN_EDVLP | GMAC_VLAN_ESVL);
950
+		value &= ~GMAC_VLAN_DOVLTC;
951
+		value &= ~GMAC_VLAN_VID;
952
+
953
+		writel(value, ioaddr + GMAC_VLAN_TAG);
954
+	}
955
+}
956
+
957
+static void dwmac4_sarc_configure(void __iomem *ioaddr, int val)
958
+{
959
+	u32 value = readl(ioaddr + GMAC_CONFIG);
960
+
961
+	value &= ~GMAC_CONFIG_SARC;
962
+	value |= val << GMAC_CONFIG_SARC_SHIFT;
963
+
964
+	writel(value, ioaddr + GMAC_CONFIG);
965
+}
966
+
967
+static void dwmac4_enable_vlan(struct mac_device_info *hw, u32 type)
968
+{
969
+	void __iomem *ioaddr = hw->pcsr;
970
+	u32 value;
971
+
972
+	value = readl(ioaddr + GMAC_VLAN_INCL);
973
+	value |= GMAC_VLAN_VLTI;
974
+	value |= GMAC_VLAN_CSVL; /* Only use SVLAN */
975
+	value &= ~GMAC_VLAN_VLC;
976
+	value |= (type << GMAC_VLAN_VLC_SHIFT) & GMAC_VLAN_VLC;
977
+	writel(value, ioaddr + GMAC_VLAN_INCL);
978
+}
979
+
980
+static void dwmac4_set_arp_offload(struct mac_device_info *hw, bool en,
981
+				   u32 addr)
982
+{
983
+	void __iomem *ioaddr = hw->pcsr;
984
+	u32 value;
985
+
986
+	writel(addr, ioaddr + GMAC_ARP_ADDR);
987
+
988
+	value = readl(ioaddr + GMAC_CONFIG);
989
+	if (en)
990
+		value |= GMAC_CONFIG_ARPEN;
991
+	else
992
+		value &= ~GMAC_CONFIG_ARPEN;
993
+	writel(value, ioaddr + GMAC_CONFIG);
994
+}
995
+
996
+static int dwmac4_config_l3_filter(struct mac_device_info *hw, u32 filter_no,
997
+				   bool en, bool ipv6, bool sa, bool inv,
998
+				   u32 match)
999
+{
1000
+	void __iomem *ioaddr = hw->pcsr;
1001
+	u32 value;
1002
+
1003
+	value = readl(ioaddr + GMAC_PACKET_FILTER);
1004
+	value |= GMAC_PACKET_FILTER_IPFE;
1005
+	writel(value, ioaddr + GMAC_PACKET_FILTER);
1006
+
1007
+	value = readl(ioaddr + GMAC_L3L4_CTRL(filter_no));
1008
+
1009
+	/* For IPv6 not both SA/DA filters can be active */
1010
+	if (ipv6) {
1011
+		value |= GMAC_L3PEN0;
1012
+		value &= ~(GMAC_L3SAM0 | GMAC_L3SAIM0);
1013
+		value &= ~(GMAC_L3DAM0 | GMAC_L3DAIM0);
1014
+		if (sa) {
1015
+			value |= GMAC_L3SAM0;
1016
+			if (inv)
1017
+				value |= GMAC_L3SAIM0;
1018
+		} else {
1019
+			value |= GMAC_L3DAM0;
1020
+			if (inv)
1021
+				value |= GMAC_L3DAIM0;
1022
+		}
1023
+	} else {
1024
+		value &= ~GMAC_L3PEN0;
1025
+		if (sa) {
1026
+			value |= GMAC_L3SAM0;
1027
+			if (inv)
1028
+				value |= GMAC_L3SAIM0;
1029
+		} else {
1030
+			value |= GMAC_L3DAM0;
1031
+			if (inv)
1032
+				value |= GMAC_L3DAIM0;
1033
+		}
1034
+	}
1035
+
1036
+	writel(value, ioaddr + GMAC_L3L4_CTRL(filter_no));
1037
+
1038
+	if (sa) {
1039
+		writel(match, ioaddr + GMAC_L3_ADDR0(filter_no));
1040
+	} else {
1041
+		writel(match, ioaddr + GMAC_L3_ADDR1(filter_no));
1042
+	}
1043
+
1044
+	if (!en)
1045
+		writel(0, ioaddr + GMAC_L3L4_CTRL(filter_no));
1046
+
1047
+	return 0;
1048
+}
1049
+
1050
+static int dwmac4_config_l4_filter(struct mac_device_info *hw, u32 filter_no,
1051
+				   bool en, bool udp, bool sa, bool inv,
1052
+				   u32 match)
1053
+{
1054
+	void __iomem *ioaddr = hw->pcsr;
1055
+	u32 value;
1056
+
1057
+	value = readl(ioaddr + GMAC_PACKET_FILTER);
1058
+	value |= GMAC_PACKET_FILTER_IPFE;
1059
+	writel(value, ioaddr + GMAC_PACKET_FILTER);
1060
+
1061
+	value = readl(ioaddr + GMAC_L3L4_CTRL(filter_no));
1062
+	if (udp) {
1063
+		value |= GMAC_L4PEN0;
1064
+	} else {
1065
+		value &= ~GMAC_L4PEN0;
1066
+	}
1067
+
1068
+	value &= ~(GMAC_L4SPM0 | GMAC_L4SPIM0);
1069
+	value &= ~(GMAC_L4DPM0 | GMAC_L4DPIM0);
1070
+	if (sa) {
1071
+		value |= GMAC_L4SPM0;
1072
+		if (inv)
1073
+			value |= GMAC_L4SPIM0;
1074
+	} else {
1075
+		value |= GMAC_L4DPM0;
1076
+		if (inv)
1077
+			value |= GMAC_L4DPIM0;
1078
+	}
1079
+
1080
+	writel(value, ioaddr + GMAC_L3L4_CTRL(filter_no));
1081
+
1082
+	if (sa) {
1083
+		value = match & GMAC_L4SP0;
1084
+	} else {
1085
+		value = (match << GMAC_L4DP0_SHIFT) & GMAC_L4DP0;
1086
+	}
1087
+
1088
+	writel(value, ioaddr + GMAC_L4_ADDR(filter_no));
1089
+
1090
+	if (!en)
1091
+		writel(0, ioaddr + GMAC_L3L4_CTRL(filter_no));
1092
+
1093
+	return 0;
1094
+}
1095
+
7181096 #ifdef CONFIG_STMMAC_FULL
7191097 const struct stmmac_ops dwmac4_ops = {
7201098 	.core_init = dwmac4_core_init,
....@@ -745,6 +1123,16 @@
7451123 	.pcs_get_adv_lp = dwmac4_get_adv_lp,
7461124 	.debug = dwmac4_debug,
7471125 	.set_filter = dwmac4_set_filter,
1126
+	.set_mac_loopback = dwmac4_set_mac_loopback,
1127
+	.update_vlan_hash = dwmac4_update_vlan_hash,
1128
+	.sarc_configure = dwmac4_sarc_configure,
1129
+	.enable_vlan = dwmac4_enable_vlan,
1130
+	.set_arp_offload = dwmac4_set_arp_offload,
1131
+	.config_l3_filter = dwmac4_config_l3_filter,
1132
+	.config_l4_filter = dwmac4_config_l4_filter,
1133
+	.add_hw_vlan_rx_fltr = dwmac4_add_hw_vlan_rx_fltr,
1134
+	.del_hw_vlan_rx_fltr = dwmac4_del_hw_vlan_rx_fltr,
1135
+	.restore_hw_vlan_rx_fltr = dwmac4_restore_hw_vlan_rx_fltr,
7481136 };
7491137 #endif
7501138 
....@@ -777,6 +1165,23 @@
7771165 	.pcs_get_adv_lp = dwmac4_get_adv_lp,
7781166 	.debug = dwmac4_debug,
7791167 	.set_filter = dwmac4_set_filter,
1168
+#ifdef CONFIG_STMMAC_FULL
1169
+	.flex_pps_config = dwmac5_flex_pps_config,
1170
+#endif
1171
+	.set_mac_loopback = dwmac4_set_mac_loopback,
1172
+	.update_vlan_hash = dwmac4_update_vlan_hash,
1173
+	.sarc_configure = dwmac4_sarc_configure,
1174
+	.enable_vlan = dwmac4_enable_vlan,
1175
+	.set_arp_offload = dwmac4_set_arp_offload,
1176
+	.config_l3_filter = dwmac4_config_l3_filter,
1177
+	.config_l4_filter = dwmac4_config_l4_filter,
1178
+#ifdef CONFIG_STMMAC_FULL
1179
+	.est_configure = dwmac5_est_configure,
1180
+	.fpe_configure = dwmac5_fpe_configure,
1181
+#endif
1182
+	.add_hw_vlan_rx_fltr = dwmac4_add_hw_vlan_rx_fltr,
1183
+	.del_hw_vlan_rx_fltr = dwmac4_del_hw_vlan_rx_fltr,
1184
+	.restore_hw_vlan_rx_fltr = dwmac4_restore_hw_vlan_rx_fltr,
7801185 };
7811186 
7821187 #ifdef CONFIG_STMMAC_FULL
....@@ -814,8 +1219,51 @@
8141219 	.safety_feat_dump = dwmac5_safety_feat_dump,
8151220 	.rxp_config = dwmac5_rxp_config,
8161221 	.flex_pps_config = dwmac5_flex_pps_config,
1222
+	.set_mac_loopback = dwmac4_set_mac_loopback,
1223
+	.update_vlan_hash = dwmac4_update_vlan_hash,
1224
+	.sarc_configure = dwmac4_sarc_configure,
1225
+	.enable_vlan = dwmac4_enable_vlan,
1226
+	.set_arp_offload = dwmac4_set_arp_offload,
1227
+	.config_l3_filter = dwmac4_config_l3_filter,
1228
+	.config_l4_filter = dwmac4_config_l4_filter,
1229
+	.est_configure = dwmac5_est_configure,
1230
+	.fpe_configure = dwmac5_fpe_configure,
1231
+	.add_hw_vlan_rx_fltr = dwmac4_add_hw_vlan_rx_fltr,
1232
+	.del_hw_vlan_rx_fltr = dwmac4_del_hw_vlan_rx_fltr,
1233
+	.restore_hw_vlan_rx_fltr = dwmac4_restore_hw_vlan_rx_fltr,
8171234 };
8181235 #endif
1236
+
1237
+static u32 dwmac4_get_num_vlan(void __iomem *ioaddr)
1238
+{
1239
+	u32 val, num_vlan;
1240
+
1241
+	val = readl(ioaddr + GMAC_HW_FEATURE3);
1242
+	switch (val & GMAC_HW_FEAT_NRVF) {
1243
+	case 0:
1244
+		num_vlan = 1;
1245
+		break;
1246
+	case 1:
1247
+		num_vlan = 4;
1248
+		break;
1249
+	case 2:
1250
+		num_vlan = 8;
1251
+		break;
1252
+	case 3:
1253
+		num_vlan = 16;
1254
+		break;
1255
+	case 4:
1256
+		num_vlan = 24;
1257
+		break;
1258
+	case 5:
1259
+		num_vlan = 32;
1260
+		break;
1261
+	default:
1262
+		num_vlan = 1;
1263
+	}
1264
+
1265
+	return num_vlan;
1266
+}
8191267 
8201268 int dwmac4_setup(struct stmmac_priv *priv)
8211269 {
....@@ -845,6 +1293,7 @@
8451293 	mac->mii.reg_mask = GENMASK(20, 16);
8461294 	mac->mii.clk_csr_shift = 8;
8471295 	mac->mii.clk_csr_mask = GENMASK(11, 8);
1296
+	mac->num_vlan = dwmac4_get_num_vlan(priv->ioaddr);
8481297 
8491298 	return 0;
8501299 }