~hc/RK356X_SDK_RELEASE.git

..	..	@@ -1,3 +1,4 @@
	1	+// SPDX-License-Identifier: GPL-2.0-only
1	2	/*
2	3	* This is the driver for the GMAC on-chip Ethernet controller for ST SoCs.
3	4	* DWC Ether MAC version 4.00 has been used for developing this code.
..	..	@@ -5,10 +6,6 @@
5	6	* This only implements the mac core functions for this chip.
6	7	*
7	8	* 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.
12	9	*
13	10	* Author: Alexandre Torgue <alexandre.torgue@st.com>
14	11	*/
..	..	@@ -28,14 +25,8 @@
28	25	{
29	26	void __iomem *ioaddr = hw->pcsr;
30	27	u32 value = readl(ioaddr + GMAC_CONFIG);
31		- int mtu = dev->mtu;
32	28
33	29	value \|= GMAC_CORE_INIT;
34		-
35		- if (mtu > 1500)
36		- value \|= GMAC_CONFIG_2K;
37		- if (mtu > 2000)
38		- value \|= GMAC_CONFIG_JE;
39	30
40	31	if (hw->ps) {
41	32	value \|= GMAC_CONFIG_TE;
..	..	@@ -196,6 +187,8 @@
196	187	default:
197	188	break;
198	189	}
	190	+
	191	+ writel(value, ioaddr + MTL_OPERATION_MODE);
199	192	}
200	193
201	194	static void dwmac4_set_mtl_tx_queue_weight(struct mac_device_info *hw,
..	..	@@ -222,6 +215,9 @@
222	215	if (queue == 0 \|\| queue == 4) {
223	216	value &= ~MTL_RXQ_DMA_Q04MDMACH_MASK;
224	217	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);
225	221	} else {
226	222	value &= ~MTL_RXQ_DMA_QXMDMACH_MASK(queue);
227	223	value \|= MTL_RXQ_DMA_QXMDMACH(chan, queue);
..	..	@@ -401,46 +397,275 @@
401	397	writel(value, ioaddr + GMAC4_LPI_TIMER_CTRL);
402	398	}
403	399
	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	+ if (hw->promisc) {
	457	+ netdev_err(dev,
	458	+ "Adding VLAN in promisc mode not supported\n");
	459	+ return -EPERM;
	460	+ }
	461	+
	462	+ /* Single Rx VLAN Filter */
	463	+ if (hw->num_vlan == 1) {
	464	+ /* For single VLAN filter, VID 0 means VLAN promiscuous */
	465	+ if (vid == 0) {
	466	+ netdev_warn(dev, "Adding VLAN ID 0 is not supported\n");
	467	+ return -EPERM;
	468	+ }
	469	+
	470	+ if (hw->vlan_filter[0] & GMAC_VLAN_TAG_VID) {
	471	+ netdev_err(dev, "Only single VLAN ID supported\n");
	472	+ return -EPERM;
	473	+ }
	474	+
	475	+ hw->vlan_filter[0] = vid;
	476	+ dwmac4_write_single_vlan(dev, vid);
	477	+
	478	+ return 0;
	479	+ }
	480	+
	481	+ /* Extended Rx VLAN Filter Enable */
	482	+ val \|= GMAC_VLAN_TAG_DATA_ETV \| GMAC_VLAN_TAG_DATA_VEN \| vid;
	483	+
	484	+ for (i = 0; i < hw->num_vlan; i++) {
	485	+ if (hw->vlan_filter[i] == val)
	486	+ return 0;
	487	+ else if (!(hw->vlan_filter[i] & GMAC_VLAN_TAG_DATA_VEN))
	488	+ index = i;
	489	+ }
	490	+
	491	+ if (index == -1) {
	492	+ netdev_err(dev, "MAC_VLAN_Tag_Filter full (size: %0u)\n",
	493	+ hw->num_vlan);
	494	+ return -EPERM;
	495	+ }
	496	+
	497	+ ret = dwmac4_write_vlan_filter(dev, hw, index, val);
	498	+
	499	+ if (!ret)
	500	+ hw->vlan_filter[index] = val;
	501	+
	502	+ return ret;
	503	+}
	504	+
	505	+static int dwmac4_del_hw_vlan_rx_fltr(struct net_device *dev,
	506	+ struct mac_device_info *hw,
	507	+ __be16 proto, u16 vid)
	508	+{
	509	+ int i, ret = 0;
	510	+
	511	+ if (hw->promisc) {
	512	+ netdev_err(dev,
	513	+ "Deleting VLAN in promisc mode not supported\n");
	514	+ return -EPERM;
	515	+ }
	516	+
	517	+ /* Single Rx VLAN Filter */
	518	+ if (hw->num_vlan == 1) {
	519	+ if ((hw->vlan_filter[0] & GMAC_VLAN_TAG_VID) == vid) {
	520	+ hw->vlan_filter[0] = 0;
	521	+ dwmac4_write_single_vlan(dev, 0);
	522	+ }
	523	+ return 0;
	524	+ }
	525	+
	526	+ /* Extended Rx VLAN Filter Enable */
	527	+ for (i = 0; i < hw->num_vlan; i++) {
	528	+ if ((hw->vlan_filter[i] & GMAC_VLAN_TAG_DATA_VID) == vid) {
	529	+ ret = dwmac4_write_vlan_filter(dev, hw, i, 0);
	530	+
	531	+ if (!ret)
	532	+ hw->vlan_filter[i] = 0;
	533	+ else
	534	+ return ret;
	535	+ }
	536	+ }
	537	+
	538	+ return ret;
	539	+}
	540	+
	541	+static void dwmac4_vlan_promisc_enable(struct net_device *dev,
	542	+ struct mac_device_info *hw)
	543	+{
	544	+ void __iomem *ioaddr = hw->pcsr;
	545	+ u32 value;
	546	+ u32 hash;
	547	+ u32 val;
	548	+ int i;
	549	+
	550	+ /* Single Rx VLAN Filter */
	551	+ if (hw->num_vlan == 1) {
	552	+ dwmac4_write_single_vlan(dev, 0);
	553	+ return;
	554	+ }
	555	+
	556	+ /* Extended Rx VLAN Filter Enable */
	557	+ for (i = 0; i < hw->num_vlan; i++) {
	558	+ if (hw->vlan_filter[i] & GMAC_VLAN_TAG_DATA_VEN) {
	559	+ val = hw->vlan_filter[i] & ~GMAC_VLAN_TAG_DATA_VEN;
	560	+ dwmac4_write_vlan_filter(dev, hw, i, val);
	561	+ }
	562	+ }
	563	+
	564	+ hash = readl(ioaddr + GMAC_VLAN_HASH_TABLE);
	565	+ if (hash & GMAC_VLAN_VLHT) {
	566	+ value = readl(ioaddr + GMAC_VLAN_TAG);
	567	+ if (value & GMAC_VLAN_VTHM) {
	568	+ value &= ~GMAC_VLAN_VTHM;
	569	+ writel(value, ioaddr + GMAC_VLAN_TAG);
	570	+ }
	571	+ }
	572	+}
	573	+
	574	+static void dwmac4_restore_hw_vlan_rx_fltr(struct net_device *dev,
	575	+ struct mac_device_info *hw)
	576	+{
	577	+ void __iomem *ioaddr = hw->pcsr;
	578	+ u32 value;
	579	+ u32 hash;
	580	+ u32 val;
	581	+ int i;
	582	+
	583	+ /* Single Rx VLAN Filter */
	584	+ if (hw->num_vlan == 1) {
	585	+ dwmac4_write_single_vlan(dev, hw->vlan_filter[0]);
	586	+ return;
	587	+ }
	588	+
	589	+ /* Extended Rx VLAN Filter Enable */
	590	+ for (i = 0; i < hw->num_vlan; i++) {
	591	+ if (hw->vlan_filter[i] & GMAC_VLAN_TAG_DATA_VEN) {
	592	+ val = hw->vlan_filter[i];
	593	+ dwmac4_write_vlan_filter(dev, hw, i, val);
	594	+ }
	595	+ }
	596	+
	597	+ hash = readl(ioaddr + GMAC_VLAN_HASH_TABLE);
	598	+ if (hash & GMAC_VLAN_VLHT) {
	599	+ value = readl(ioaddr + GMAC_VLAN_TAG);
	600	+ value \|= GMAC_VLAN_VTHM;
	601	+ writel(value, ioaddr + GMAC_VLAN_TAG);
	602	+ }
	603	+}
	604	+
404	605	static void dwmac4_set_filter(struct mac_device_info *hw,
405	606	struct net_device *dev)
406	607	{
407	608	void __iomem ioaddr = (void __iomem )dev->base_addr;
408		- unsigned int value = 0;
	609	+ int numhashregs = (hw->multicast_filter_bins >> 5);
	610	+ int mcbitslog2 = hw->mcast_bits_log2;
	611	+ unsigned int value;
	612	+ u32 mc_filter[8];
	613	+ int i;
409	614
	615	+ memset(mc_filter, 0, sizeof(mc_filter));
	616	+
	617	+ value = readl(ioaddr + GMAC_PACKET_FILTER);
	618	+ value &= ~GMAC_PACKET_FILTER_HMC;
	619	+ value &= ~GMAC_PACKET_FILTER_HPF;
	620	+ value &= ~GMAC_PACKET_FILTER_PCF;
	621	+ value &= ~GMAC_PACKET_FILTER_PM;
	622	+ value &= ~GMAC_PACKET_FILTER_PR;
	623	+ value &= ~GMAC_PACKET_FILTER_RA;
410	624	if (dev->flags & IFF_PROMISC) {
411		- value = GMAC_PACKET_FILTER_PR;
	625	+ /* VLAN Tag Filter Fail Packets Queuing */
	626	+ if (hw->vlan_fail_q_en) {
	627	+ value = readl(ioaddr + GMAC_RXQ_CTRL4);
	628	+ value &= ~GMAC_RXQCTRL_VFFQ_MASK;
	629	+ value \|= GMAC_RXQCTRL_VFFQE \|
	630	+ (hw->vlan_fail_q << GMAC_RXQCTRL_VFFQ_SHIFT);
	631	+ writel(value, ioaddr + GMAC_RXQ_CTRL4);
	632	+ value = GMAC_PACKET_FILTER_PR \| GMAC_PACKET_FILTER_RA;
	633	+ } else {
	634	+ value = GMAC_PACKET_FILTER_PR \| GMAC_PACKET_FILTER_PCF;
	635	+ }
	636	+
412	637	} else if ((dev->flags & IFF_ALLMULTI) \|\|
413		- (netdev_mc_count(dev) > HASH_TABLE_SIZE)) {
	638	+ (netdev_mc_count(dev) > hw->multicast_filter_bins)) {
414	639	/* 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];
	640	+ value \|= GMAC_PACKET_FILTER_PM;
	641	+ /* Set all the bits of the HASH tab */
	642	+ memset(mc_filter, 0xff, sizeof(mc_filter));
	643	+ } else if (!netdev_mc_empty(dev) && (dev->flags & IFF_MULTICAST)) {
423	644	struct netdev_hw_addr *ha;
424	645
425	646	/* Hash filter for multicast */
426		- value = GMAC_PACKET_FILTER_HMC;
	647	+ value \|= GMAC_PACKET_FILTER_HMC;
427	648
428		- memset(mc_filter, 0, sizeof(mc_filter));
429	649	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.
	650	+ /* The upper n bits of the calculated CRC are used to
	651	+ * index the contents of the hash table. The number of
	652	+ * bits used depends on the hardware configuration
	653	+ * selected at core configuration time.
432	654	*/
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
	655	+ u32 bit_nr = bitrev32(~crc32_le(~0, ha->addr,
	656	+ ETH_ALEN)) >> (32 - mcbitslog2);
	657	+ /* The most significant bit determines the register to
	658	+ * use (H/L) while the other 5 bits determine the bit
	659	+ * within the register.
438	660	*/
439		- mc_filter[bit_nr >> 5] \|= (1 << (bit_nr & 0x1F));
	661	+ mc_filter[bit_nr >> 5] \|= (1 << (bit_nr & 0x1f));
440	662	}
441		- writel(mc_filter[0], ioaddr + GMAC_HASH_TAB_0_31);
442		- writel(mc_filter[1], ioaddr + GMAC_HASH_TAB_32_63);
443	663	}
	664	+
	665	+ for (i = 0; i < numhashregs; i++)
	666	+ writel(mc_filter[i], ioaddr + GMAC_HASH_TAB(i));
	667	+
	668	+ value \|= GMAC_PACKET_FILTER_HPF;
444	669
445	670	/* Handle multiple unicast addresses */
446	671	if (netdev_uc_count(dev) > hw->unicast_filter_entries) {
..	..	@@ -457,14 +682,30 @@
457	682	reg++;
458	683	}
459	684
460		- while (reg <= GMAC_MAX_PERFECT_ADDRESSES) {
	685	+ while (reg < GMAC_MAX_PERFECT_ADDRESSES) {
461	686	writel(0, ioaddr + GMAC_ADDR_HIGH(reg));
462	687	writel(0, ioaddr + GMAC_ADDR_LOW(reg));
463	688	reg++;
464	689	}
465	690	}
466	691
	692	+ /* VLAN filtering */
	693	+ if (dev->features & NETIF_F_HW_VLAN_CTAG_FILTER)
	694	+ value \|= GMAC_PACKET_FILTER_VTFE;
	695	+
467	696	writel(value, ioaddr + GMAC_PACKET_FILTER);
	697	+
	698	+ if (dev->flags & IFF_PROMISC && !hw->vlan_fail_q_en) {
	699	+ if (!hw->promisc) {
	700	+ hw->promisc = 1;
	701	+ dwmac4_vlan_promisc_enable(dev, hw);
	702	+ }
	703	+ } else {
	704	+ if (hw->promisc) {
	705	+ hw->promisc = 0;
	706	+ dwmac4_restore_hw_vlan_rx_fltr(dev, hw);
	707	+ }
	708	+ }
468	709	}
469	710
470	711	static void dwmac4_flow_ctrl(struct mac_device_info *hw, unsigned int duplex,
..	..	@@ -479,6 +720,8 @@
479	720	if (fc & FLOW_RX) {
480	721	pr_debug("\tReceive Flow-Control ON\n");
481	722	flow \|= GMAC_RX_FLOW_CTRL_RFE;
	723	+ } else {
	724	+ pr_debug("\tReceive Flow-Control OFF\n");
482	725	}
483	726	writel(flow, ioaddr + GMAC_RX_FLOW_CTRL);
484	727
..	..	@@ -715,6 +958,197 @@
715	958	x->mac_gmii_rx_proto_engine++;
716	959	}
717	960
	961	+static void dwmac4_set_mac_loopback(void __iomem *ioaddr, bool enable)
	962	+{
	963	+ u32 value = readl(ioaddr + GMAC_CONFIG);
	964	+
	965	+ if (enable)
	966	+ value \|= GMAC_CONFIG_LM;
	967	+ else
	968	+ value &= ~GMAC_CONFIG_LM;
	969	+
	970	+ writel(value, ioaddr + GMAC_CONFIG);
	971	+}
	972	+
	973	+static void dwmac4_update_vlan_hash(struct mac_device_info *hw, u32 hash,
	974	+ __le16 perfect_match, bool is_double)
	975	+{
	976	+ void __iomem *ioaddr = hw->pcsr;
	977	+ u32 value;
	978	+
	979	+ writel(hash, ioaddr + GMAC_VLAN_HASH_TABLE);
	980	+
	981	+ value = readl(ioaddr + GMAC_VLAN_TAG);
	982	+
	983	+ if (hash) {
	984	+ value \|= GMAC_VLAN_VTHM \| GMAC_VLAN_ETV;
	985	+ if (is_double) {
	986	+ value \|= GMAC_VLAN_EDVLP;
	987	+ value \|= GMAC_VLAN_ESVL;
	988	+ value \|= GMAC_VLAN_DOVLTC;
	989	+ }
	990	+
	991	+ writel(value, ioaddr + GMAC_VLAN_TAG);
	992	+ } else if (perfect_match) {
	993	+ u32 value = GMAC_VLAN_ETV;
	994	+
	995	+ if (is_double) {
	996	+ value \|= GMAC_VLAN_EDVLP;
	997	+ value \|= GMAC_VLAN_ESVL;
	998	+ value \|= GMAC_VLAN_DOVLTC;
	999	+ }
	1000	+
	1001	+ writel(value \| perfect_match, ioaddr + GMAC_VLAN_TAG);
	1002	+ } else {
	1003	+ value &= ~(GMAC_VLAN_VTHM \| GMAC_VLAN_ETV);
	1004	+ value &= ~(GMAC_VLAN_EDVLP \| GMAC_VLAN_ESVL);
	1005	+ value &= ~GMAC_VLAN_DOVLTC;
	1006	+ value &= ~GMAC_VLAN_VID;
	1007	+
	1008	+ writel(value, ioaddr + GMAC_VLAN_TAG);
	1009	+ }
	1010	+}
	1011	+
	1012	+static void dwmac4_sarc_configure(void __iomem *ioaddr, int val)
	1013	+{
	1014	+ u32 value = readl(ioaddr + GMAC_CONFIG);
	1015	+
	1016	+ value &= ~GMAC_CONFIG_SARC;
	1017	+ value \|= val << GMAC_CONFIG_SARC_SHIFT;
	1018	+
	1019	+ writel(value, ioaddr + GMAC_CONFIG);
	1020	+}
	1021	+
	1022	+static void dwmac4_enable_vlan(struct mac_device_info *hw, u32 type)
	1023	+{
	1024	+ void __iomem *ioaddr = hw->pcsr;
	1025	+ u32 value;
	1026	+
	1027	+ value = readl(ioaddr + GMAC_VLAN_INCL);
	1028	+ value \|= GMAC_VLAN_VLTI;
	1029	+ value \|= GMAC_VLAN_CSVL; /* Only use SVLAN */
	1030	+ value &= ~GMAC_VLAN_VLC;
	1031	+ value \|= (type << GMAC_VLAN_VLC_SHIFT) & GMAC_VLAN_VLC;
	1032	+ writel(value, ioaddr + GMAC_VLAN_INCL);
	1033	+}
	1034	+
	1035	+static void dwmac4_set_arp_offload(struct mac_device_info *hw, bool en,
	1036	+ u32 addr)
	1037	+{
	1038	+ void __iomem *ioaddr = hw->pcsr;
	1039	+ u32 value;
	1040	+
	1041	+ writel(addr, ioaddr + GMAC_ARP_ADDR);
	1042	+
	1043	+ value = readl(ioaddr + GMAC_CONFIG);
	1044	+ if (en)
	1045	+ value \|= GMAC_CONFIG_ARPEN;
	1046	+ else
	1047	+ value &= ~GMAC_CONFIG_ARPEN;
	1048	+ writel(value, ioaddr + GMAC_CONFIG);
	1049	+}
	1050	+
	1051	+static int dwmac4_config_l3_filter(struct mac_device_info *hw, u32 filter_no,
	1052	+ bool en, bool ipv6, bool sa, bool inv,
	1053	+ u32 match)
	1054	+{
	1055	+ void __iomem *ioaddr = hw->pcsr;
	1056	+ u32 value;
	1057	+
	1058	+ value = readl(ioaddr + GMAC_PACKET_FILTER);
	1059	+ value \|= GMAC_PACKET_FILTER_IPFE;
	1060	+ writel(value, ioaddr + GMAC_PACKET_FILTER);
	1061	+
	1062	+ value = readl(ioaddr + GMAC_L3L4_CTRL(filter_no));
	1063	+
	1064	+ /* For IPv6 not both SA/DA filters can be active */
	1065	+ if (ipv6) {
	1066	+ value \|= GMAC_L3PEN0;
	1067	+ value &= ~(GMAC_L3SAM0 \| GMAC_L3SAIM0);
	1068	+ value &= ~(GMAC_L3DAM0 \| GMAC_L3DAIM0);
	1069	+ if (sa) {
	1070	+ value \|= GMAC_L3SAM0;
	1071	+ if (inv)
	1072	+ value \|= GMAC_L3SAIM0;
	1073	+ } else {
	1074	+ value \|= GMAC_L3DAM0;
	1075	+ if (inv)
	1076	+ value \|= GMAC_L3DAIM0;
	1077	+ }
	1078	+ } else {
	1079	+ value &= ~GMAC_L3PEN0;
	1080	+ if (sa) {
	1081	+ value \|= GMAC_L3SAM0;
	1082	+ if (inv)
	1083	+ value \|= GMAC_L3SAIM0;
	1084	+ } else {
	1085	+ value \|= GMAC_L3DAM0;
	1086	+ if (inv)
	1087	+ value \|= GMAC_L3DAIM0;
	1088	+ }
	1089	+ }
	1090	+
	1091	+ writel(value, ioaddr + GMAC_L3L4_CTRL(filter_no));
	1092	+
	1093	+ if (sa) {
	1094	+ writel(match, ioaddr + GMAC_L3_ADDR0(filter_no));
	1095	+ } else {
	1096	+ writel(match, ioaddr + GMAC_L3_ADDR1(filter_no));
	1097	+ }
	1098	+
	1099	+ if (!en)
	1100	+ writel(0, ioaddr + GMAC_L3L4_CTRL(filter_no));
	1101	+
	1102	+ return 0;
	1103	+}
	1104	+
	1105	+static int dwmac4_config_l4_filter(struct mac_device_info *hw, u32 filter_no,
	1106	+ bool en, bool udp, bool sa, bool inv,
	1107	+ u32 match)
	1108	+{
	1109	+ void __iomem *ioaddr = hw->pcsr;
	1110	+ u32 value;
	1111	+
	1112	+ value = readl(ioaddr + GMAC_PACKET_FILTER);
	1113	+ value \|= GMAC_PACKET_FILTER_IPFE;
	1114	+ writel(value, ioaddr + GMAC_PACKET_FILTER);
	1115	+
	1116	+ value = readl(ioaddr + GMAC_L3L4_CTRL(filter_no));
	1117	+ if (udp) {
	1118	+ value \|= GMAC_L4PEN0;
	1119	+ } else {
	1120	+ value &= ~GMAC_L4PEN0;
	1121	+ }
	1122	+
	1123	+ value &= ~(GMAC_L4SPM0 \| GMAC_L4SPIM0);
	1124	+ value &= ~(GMAC_L4DPM0 \| GMAC_L4DPIM0);
	1125	+ if (sa) {
	1126	+ value \|= GMAC_L4SPM0;
	1127	+ if (inv)
	1128	+ value \|= GMAC_L4SPIM0;
	1129	+ } else {
	1130	+ value \|= GMAC_L4DPM0;
	1131	+ if (inv)
	1132	+ value \|= GMAC_L4DPIM0;
	1133	+ }
	1134	+
	1135	+ writel(value, ioaddr + GMAC_L3L4_CTRL(filter_no));
	1136	+
	1137	+ if (sa) {
	1138	+ value = match & GMAC_L4SP0;
	1139	+ } else {
	1140	+ value = (match << GMAC_L4DP0_SHIFT) & GMAC_L4DP0;
	1141	+ }
	1142	+
	1143	+ writel(value, ioaddr + GMAC_L4_ADDR(filter_no));
	1144	+
	1145	+ if (!en)
	1146	+ writel(0, ioaddr + GMAC_L3L4_CTRL(filter_no));
	1147	+
	1148	+ return 0;
	1149	+}
	1150	+
	1151	+#ifdef CONFIG_STMMAC_FULL
718	1152	const struct stmmac_ops dwmac4_ops = {
719	1153	.core_init = dwmac4_core_init,
720	1154	.set_mac = stmmac_set_mac,
..	..	@@ -744,7 +1178,18 @@
744	1178	.pcs_get_adv_lp = dwmac4_get_adv_lp,
745	1179	.debug = dwmac4_debug,
746	1180	.set_filter = dwmac4_set_filter,
	1181	+ .set_mac_loopback = dwmac4_set_mac_loopback,
	1182	+ .update_vlan_hash = dwmac4_update_vlan_hash,
	1183	+ .sarc_configure = dwmac4_sarc_configure,
	1184	+ .enable_vlan = dwmac4_enable_vlan,
	1185	+ .set_arp_offload = dwmac4_set_arp_offload,
	1186	+ .config_l3_filter = dwmac4_config_l3_filter,
	1187	+ .config_l4_filter = dwmac4_config_l4_filter,
	1188	+ .add_hw_vlan_rx_fltr = dwmac4_add_hw_vlan_rx_fltr,
	1189	+ .del_hw_vlan_rx_fltr = dwmac4_del_hw_vlan_rx_fltr,
	1190	+ .restore_hw_vlan_rx_fltr = dwmac4_restore_hw_vlan_rx_fltr,
747	1191	};
	1192	+#endif
748	1193
749	1194	const struct stmmac_ops dwmac410_ops = {
750	1195	.core_init = dwmac4_core_init,
..	..	@@ -775,8 +1220,26 @@
775	1220	.pcs_get_adv_lp = dwmac4_get_adv_lp,
776	1221	.debug = dwmac4_debug,
777	1222	.set_filter = dwmac4_set_filter,
	1223	+#ifdef CONFIG_STMMAC_FULL
	1224	+ .flex_pps_config = dwmac5_flex_pps_config,
	1225	+#endif
	1226	+ .set_mac_loopback = dwmac4_set_mac_loopback,
	1227	+ .update_vlan_hash = dwmac4_update_vlan_hash,
	1228	+ .sarc_configure = dwmac4_sarc_configure,
	1229	+ .enable_vlan = dwmac4_enable_vlan,
	1230	+ .set_arp_offload = dwmac4_set_arp_offload,
	1231	+ .config_l3_filter = dwmac4_config_l3_filter,
	1232	+ .config_l4_filter = dwmac4_config_l4_filter,
	1233	+#ifdef CONFIG_STMMAC_FULL
	1234	+ .est_configure = dwmac5_est_configure,
	1235	+ .fpe_configure = dwmac5_fpe_configure,
	1236	+#endif
	1237	+ .add_hw_vlan_rx_fltr = dwmac4_add_hw_vlan_rx_fltr,
	1238	+ .del_hw_vlan_rx_fltr = dwmac4_del_hw_vlan_rx_fltr,
	1239	+ .restore_hw_vlan_rx_fltr = dwmac4_restore_hw_vlan_rx_fltr,
778	1240	};
779	1241
	1242	+#ifdef CONFIG_STMMAC_FULL
780	1243	const struct stmmac_ops dwmac510_ops = {
781	1244	.core_init = dwmac4_core_init,
782	1245	.set_mac = stmmac_dwmac4_set_mac,
..	..	@@ -811,7 +1274,51 @@
811	1274	.safety_feat_dump = dwmac5_safety_feat_dump,
812	1275	.rxp_config = dwmac5_rxp_config,
813	1276	.flex_pps_config = dwmac5_flex_pps_config,
	1277	+ .set_mac_loopback = dwmac4_set_mac_loopback,
	1278	+ .update_vlan_hash = dwmac4_update_vlan_hash,
	1279	+ .sarc_configure = dwmac4_sarc_configure,
	1280	+ .enable_vlan = dwmac4_enable_vlan,
	1281	+ .set_arp_offload = dwmac4_set_arp_offload,
	1282	+ .config_l3_filter = dwmac4_config_l3_filter,
	1283	+ .config_l4_filter = dwmac4_config_l4_filter,
	1284	+ .est_configure = dwmac5_est_configure,
	1285	+ .fpe_configure = dwmac5_fpe_configure,
	1286	+ .add_hw_vlan_rx_fltr = dwmac4_add_hw_vlan_rx_fltr,
	1287	+ .del_hw_vlan_rx_fltr = dwmac4_del_hw_vlan_rx_fltr,
	1288	+ .restore_hw_vlan_rx_fltr = dwmac4_restore_hw_vlan_rx_fltr,
814	1289	};
	1290	+#endif
	1291	+
	1292	+static u32 dwmac4_get_num_vlan(void __iomem *ioaddr)
	1293	+{
	1294	+ u32 val, num_vlan;
	1295	+
	1296	+ val = readl(ioaddr + GMAC_HW_FEATURE3);
	1297	+ switch (val & GMAC_HW_FEAT_NRVF) {
	1298	+ case 0:
	1299	+ num_vlan = 1;
	1300	+ break;
	1301	+ case 1:
	1302	+ num_vlan = 4;
	1303	+ break;
	1304	+ case 2:
	1305	+ num_vlan = 8;
	1306	+ break;
	1307	+ case 3:
	1308	+ num_vlan = 16;
	1309	+ break;
	1310	+ case 4:
	1311	+ num_vlan = 24;
	1312	+ break;
	1313	+ case 5:
	1314	+ num_vlan = 32;
	1315	+ break;
	1316	+ default:
	1317	+ num_vlan = 1;
	1318	+ }
	1319	+
	1320	+ return num_vlan;
	1321	+}
815	1322
816	1323	int dwmac4_setup(struct stmmac_priv *priv)
817	1324	{
..	..	@@ -841,6 +1348,7 @@
841	1348	mac->mii.reg_mask = GENMASK(20, 16);
842	1349	mac->mii.clk_csr_shift = 8;
843	1350	mac->mii.clk_csr_mask = GENMASK(11, 8);
	1351	+ mac->num_vlan = dwmac4_get_num_vlan(priv->ioaddr);
844	1352
845	1353	return 0;
846	1354	}