~hc/RK356X_SDK_RELEASE.git

..	..	@@ -1,20 +1,10 @@
	1	+// SPDX-License-Identifier: GPL-2.0-only
1	2	/*******************************************************************************
2	3	This is the driver for the ST MAC 10/100/1000 on-chip Ethernet controllers.
3	4	ST Ethernet IPs are built around a Synopsys IP Core.
4	5
5	6	Copyright(C) 2007-2011 STMicroelectronics Ltd
6	7
7		- This program is free software; you can redistribute it and/or modify it
8		- under the terms and conditions of the GNU General Public License,
9		- version 2, as published by the Free Software Foundation.
10		-
11		- This program is distributed in the hope it will be useful, but WITHOUT
12		- ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
13		- FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
14		- more details.
15		-
16		- The full GNU General Public License is included in this distribution in
17		- the file called "COPYING".
18	8
19	9	Author: Giuseppe Cavallaro <peppe.cavallaro@st.com>
20	10
..	..	@@ -38,6 +28,7 @@
38	28	#include <linux/if_vlan.h>
39	29	#include <linux/dma-mapping.h>
40	30	#include <linux/slab.h>
	31	+#include <linux/pm_runtime.h>
41	32	#include <linux/prefetch.h>
42	33	#include <linux/pinctrl/consumer.h>
43	34	#ifdef CONFIG_DEBUG_FS
..	..	@@ -45,6 +36,7 @@
45	36	#include <linux/seq_file.h>
46	37	#endif /* CONFIG_DEBUG_FS */
47	38	#include <linux/net_tstamp.h>
	39	+#include <linux/phylink.h>
48	40	#include <linux/udp.h>
49	41	#include <net/pkt_cls.h>
50	42	#include "stmmac_ptp.h"
..	..	@@ -54,6 +46,13 @@
54	46	#include "dwmac1000.h"
55	47	#include "dwxgmac2.h"
56	48	#include "hwif.h"
	49	+
	50	+/* As long as the interface is active, we keep the timestamping counter enabled
	51	+ * with fine resolution and binary rollover. This avoid non-monotonic behavior
	52	+ * (clock jumps) when changing timestamping settings at runtime.
	53	+ */
	54	+#define STMMAC_HWTS_ACTIVE (PTP_TCR_TSENA \| PTP_TCR_TSCFUPDT \| \
	55	+ PTP_TCR_TSCTRLSSR)
57	56
58	57	#define STMMAC_ALIGN(x) ALIGN(ALIGN(x, SMP_CACHE_BYTES), 16)
59	58	#define TSO_MAX_BUFF_SIZE (SZ_16K - 1)
..	..	@@ -72,10 +71,10 @@
72	71	module_param(phyaddr, int, 0444);
73	72	MODULE_PARM_DESC(phyaddr, "Physical device address");
74	73
75		-#define STMMAC_TX_THRESH (DMA_TX_SIZE / 4)
76		-#define STMMAC_RX_THRESH (DMA_RX_SIZE / 4)
	74	+#define STMMAC_TX_THRESH(x) ((x)->dma_tx_size / 4)
	75	+#define STMMAC_RX_THRESH(x) ((x)->dma_rx_size / 4)
77	76
78		-static int flow_ctrl = FLOW_OFF;
	77	+static int flow_ctrl = FLOW_AUTO;
79	78	module_param(flow_ctrl, int, 0644);
80	79	MODULE_PARM_DESC(flow_ctrl, "Flow control ability [on/off]");
81	80
..	..	@@ -103,7 +102,7 @@
103	102	static int eee_timer = STMMAC_DEFAULT_LPI_TIMER;
104	103	module_param(eee_timer, int, 0644);
105	104	MODULE_PARM_DESC(eee_timer, "LPI tx expiration time in msec");
106		-#define STMMAC_LPI_T(x) (jiffies + msecs_to_jiffies(x))
	105	+#define STMMAC_LPI_T(x) (jiffies + usecs_to_jiffies(x))
107	106
108	107	/* By default the driver will use the ring mode to manage tx and rx descriptors,
109	108	* but allow user to force to use the chain instead of the ring
..	..	@@ -115,11 +114,34 @@
115	114	static irqreturn_t stmmac_interrupt(int irq, void *dev_id);
116	115
117	116	#ifdef CONFIG_DEBUG_FS
118		-static int stmmac_init_fs(struct net_device *dev);
	117	+static const struct net_device_ops stmmac_netdev_ops;
	118	+static void stmmac_init_fs(struct net_device *dev);
119	119	static void stmmac_exit_fs(struct net_device *dev);
120	120	#endif
121	121
122	122	#define STMMAC_COAL_TIMER(x) (jiffies + usecs_to_jiffies(x))
	123	+
	124	+int stmmac_bus_clks_config(struct stmmac_priv *priv, bool enabled)
	125	+{
	126	+ int ret = 0;
	127	+
	128	+ if (enabled) {
	129	+ ret = clk_prepare_enable(priv->plat->stmmac_clk);
	130	+ if (ret)
	131	+ return ret;
	132	+ ret = clk_prepare_enable(priv->plat->pclk);
	133	+ if (ret) {
	134	+ clk_disable_unprepare(priv->plat->stmmac_clk);
	135	+ return ret;
	136	+ }
	137	+ } else {
	138	+ clk_disable_unprepare(priv->plat->stmmac_clk);
	139	+ clk_disable_unprepare(priv->plat->pclk);
	140	+ }
	141	+
	142	+ return ret;
	143	+}
	144	+EXPORT_SYMBOL_GPL(stmmac_bus_clks_config);
123	145
124	146	/**
125	147	* stmmac_verify_args - verify the driver parameters.
..	..	@@ -156,7 +178,10 @@
156	178	for (queue = 0; queue < maxq; queue++) {
157	179	struct stmmac_channel *ch = &priv->channel[queue];
158	180
159		- napi_disable(&ch->napi);
	181	+ if (queue < rx_queues_cnt)
	182	+ napi_disable(&ch->rx_napi);
	183	+ if (queue < tx_queues_cnt)
	184	+ napi_disable(&ch->tx_napi);
160	185	}
161	186	}
162	187
..	..	@@ -174,7 +199,10 @@
174	199	for (queue = 0; queue < maxq; queue++) {
175	200	struct stmmac_channel *ch = &priv->channel[queue];
176	201
177		- napi_enable(&ch->napi);
	202	+ if (queue < rx_queues_cnt)
	203	+ napi_enable(&ch->rx_napi);
	204	+ if (queue < tx_queues_cnt)
	205	+ napi_enable(&ch->tx_napi);
178	206	}
179	207	}
180	208
..	..	@@ -228,7 +256,7 @@
228	256	priv->clk_csr = STMMAC_CSR_100_150M;
229	257	else if ((clk_rate >= CSR_F_150M) && (clk_rate < CSR_F_250M))
230	258	priv->clk_csr = STMMAC_CSR_150_250M;
231		- else if ((clk_rate >= CSR_F_250M) && (clk_rate < CSR_F_300M))
	259	+ else if ((clk_rate >= CSR_F_250M) && (clk_rate <= CSR_F_300M))
232	260	priv->clk_csr = STMMAC_CSR_250_300M;
233	261	}
234	262
..	..	@@ -273,7 +301,7 @@
273	301	if (tx_q->dirty_tx > tx_q->cur_tx)
274	302	avail = tx_q->dirty_tx - tx_q->cur_tx - 1;
275	303	else
276		- avail = DMA_TX_SIZE - tx_q->cur_tx + tx_q->dirty_tx - 1;
	304	+ avail = priv->dma_tx_size - tx_q->cur_tx + tx_q->dirty_tx - 1;
277	305
278	306	return avail;
279	307	}
..	..	@@ -291,24 +319,9 @@
291	319	if (rx_q->dirty_rx <= rx_q->cur_rx)
292	320	dirty = rx_q->cur_rx - rx_q->dirty_rx;
293	321	else
294		- dirty = DMA_RX_SIZE - rx_q->dirty_rx + rx_q->cur_rx;
	322	+ dirty = priv->dma_rx_size - rx_q->dirty_rx + rx_q->cur_rx;
295	323
296	324	return dirty;
297		-}
298		-
299		-/**
300		- * stmmac_hw_fix_mac_speed - callback for speed selection
301		- * @priv: driver private structure
302		- * Description: on some platforms (e.g. ST), some HW system configuration
303		- * registers have to be set according to the link speed negotiated.
304		- */
305		-static inline void stmmac_hw_fix_mac_speed(struct stmmac_priv *priv)
306		-{
307		- struct net_device *ndev = priv->dev;
308		- struct phy_device *phydev = ndev->phydev;
309		-
310		- if (likely(priv->plat->fix_mac_speed))
311		- priv->plat->fix_mac_speed(priv->plat->bsp_priv, phydev->speed);
312	325	}
313	326
314	327	/**
..	..	@@ -351,7 +364,7 @@
351	364
352	365	/**
353	366	* stmmac_eee_ctrl_timer - EEE TX SW timer.
354		- * @arg : data hook
	367	+ * @t: timer_list struct containing private info
355	368	* Description:
356	369	* if there is no data transfer and if we are not in LPI state,
357	370	* then MAC Transmitter can be moved to LPI state.
..	..	@@ -361,7 +374,7 @@
361	374	struct stmmac_priv *priv = from_timer(priv, t, eee_ctrl_timer);
362	375
363	376	stmmac_enable_eee_mode(priv);
364		- mod_timer(&priv->eee_ctrl_timer, STMMAC_LPI_T(eee_timer));
	377	+ mod_timer(&priv->eee_ctrl_timer, STMMAC_LPI_T(priv->tx_lpi_timer));
365	378	}
366	379
367	380	/**
..	..	@@ -374,67 +387,43 @@
374	387	*/
375	388	bool stmmac_eee_init(struct stmmac_priv *priv)
376	389	{
377		- struct net_device *ndev = priv->dev;
378		- int interface = priv->plat->interface;
379		- bool ret = false;
380		-
381		- if ((interface != PHY_INTERFACE_MODE_MII) &&
382		- (interface != PHY_INTERFACE_MODE_GMII) &&
383		- !phy_interface_mode_is_rgmii(interface))
384		- goto out;
	390	+ int eee_tw_timer = priv->eee_tw_timer;
385	391
386	392	/* Using PCS we cannot dial with the phy registers at this stage
387	393	* so we do not support extra feature like EEE.
388	394	*/
389		- if ((priv->hw->pcs == STMMAC_PCS_RGMII) \|\|
390		- (priv->hw->pcs == STMMAC_PCS_TBI) \|\|
391		- (priv->hw->pcs == STMMAC_PCS_RTBI))
392		- goto out;
	395	+ if (priv->hw->pcs == STMMAC_PCS_TBI \|\|
	396	+ priv->hw->pcs == STMMAC_PCS_RTBI)
	397	+ return false;
393	398
394		- /* MAC core supports the EEE feature. */
395		- if (priv->dma_cap.eee) {
396		- int tx_lpi_timer = priv->tx_lpi_timer;
	399	+ /* Check if MAC core supports the EEE feature. */
	400	+ if (!priv->dma_cap.eee)
	401	+ return false;
397	402
398		- /* Check if the PHY supports EEE */
399		- if (phy_init_eee(ndev->phydev, 1)) {
400		- /* To manage at run-time if the EEE cannot be supported
401		- * anymore (for example because the lp caps have been
402		- * changed).
403		- * In that case the driver disable own timers.
404		- */
405		- mutex_lock(&priv->lock);
406		- if (priv->eee_active) {
407		- netdev_dbg(priv->dev, "disable EEE\n");
408		- del_timer_sync(&priv->eee_ctrl_timer);
409		- stmmac_set_eee_timer(priv, priv->hw, 0,
410		- tx_lpi_timer);
411		- }
412		- priv->eee_active = 0;
413		- mutex_unlock(&priv->lock);
414		- goto out;
	403	+ mutex_lock(&priv->lock);
	404	+
	405	+ /* Check if it needs to be deactivated */
	406	+ if (!priv->eee_active) {
	407	+ if (priv->eee_enabled) {
	408	+ netdev_dbg(priv->dev, "disable EEE\n");
	409	+ del_timer_sync(&priv->eee_ctrl_timer);
	410	+ stmmac_set_eee_timer(priv, priv->hw, 0, eee_tw_timer);
415	411	}
416		- /* Activate the EEE and start timers */
417		- mutex_lock(&priv->lock);
418		- if (!priv->eee_active) {
419		- priv->eee_active = 1;
420		- timer_setup(&priv->eee_ctrl_timer,
421		- stmmac_eee_ctrl_timer, 0);
422		- mod_timer(&priv->eee_ctrl_timer,
423		- STMMAC_LPI_T(eee_timer));
424		-
425		- stmmac_set_eee_timer(priv, priv->hw,
426		- STMMAC_DEFAULT_LIT_LS, tx_lpi_timer);
427		- }
428		- /* Set HW EEE according to the speed */
429		- stmmac_set_eee_pls(priv, priv->hw, ndev->phydev->link);
430		-
431		- ret = true;
432	412	mutex_unlock(&priv->lock);
433		-
434		- netdev_dbg(priv->dev, "Energy-Efficient Ethernet initialized\n");
	413	+ return false;
435	414	}
436		-out:
437		- return ret;
	415	+
	416	+ if (priv->eee_active && !priv->eee_enabled) {
	417	+ timer_setup(&priv->eee_ctrl_timer, stmmac_eee_ctrl_timer, 0);
	418	+ stmmac_set_eee_timer(priv, priv->hw, STMMAC_DEFAULT_LIT_LS,
	419	+ eee_tw_timer);
	420	+ }
	421	+
	422	+ mod_timer(&priv->eee_ctrl_timer, STMMAC_LPI_T(priv->tx_lpi_timer));
	423	+
	424	+ mutex_unlock(&priv->lock);
	425	+ netdev_dbg(priv->dev, "Energy-Efficient Ethernet initialized\n");
	426	+ return true;
438	427	}
439	428
440	429	/* stmmac_get_tx_hwtstamp - get HW TX timestamps
..	..	@@ -449,6 +438,7 @@
449	438	struct dma_desc p, struct sk_buff skb)
450	439	{
451	440	struct skb_shared_hwtstamps shhwtstamp;
	441	+ bool found = false;
452	442	u64 ns = 0;
453	443
454	444	if (!priv->hwts_tx_en)
..	..	@@ -460,9 +450,13 @@
460	450
461	451	/* check tx tstamp status */
462	452	if (stmmac_get_tx_timestamp_status(priv, p)) {
463		- /* get the valid tstamp */
464	453	stmmac_get_timestamp(priv, p, priv->adv_ts, &ns);
	454	+ found = true;
	455	+ } else if (!stmmac_get_mac_tx_timestamp(priv, priv->hw, &ns)) {
	456	+ found = true;
	457	+ }
465	458
	459	+ if (found) {
466	460	memset(&shhwtstamp, 0, sizeof(struct skb_shared_hwtstamps));
467	461	shhwtstamp.hwtstamp = ns_to_ktime(ns);
468	462
..	..	@@ -470,8 +464,6 @@
470	464	/* pass tstamp to stack */
471	465	skb_tstamp_tx(skb, &shhwtstamp);
472	466	}
473		-
474		- return;
475	467	}
476	468
477	469	/* stmmac_get_rx_hwtstamp - get HW RX timestamps
..	..	@@ -523,8 +515,6 @@
523	515	{
524	516	struct stmmac_priv *priv = netdev_priv(dev);
525	517	struct hwtstamp_config config;
526		- struct timespec64 now;
527		- u64 temp = 0;
528	518	u32 ptp_v2 = 0;
529	519	u32 tstamp_all = 0;
530	520	u32 ptp_over_ipv4_udp = 0;
..	..	@@ -533,11 +523,6 @@
533	523	u32 snap_type_sel = 0;
534	524	u32 ts_master_en = 0;
535	525	u32 ts_event_en = 0;
536		- u32 sec_inc = 0;
537		- u32 value = 0;
538		- bool xmac;
539		-
540		- xmac = priv->plat->has_gmac4 \|\| priv->plat->has_xgmac;
541	526
542	527	if (!(priv->dma_cap.time_stamp \|\| priv->adv_ts)) {
543	528	netdev_alert(priv->dev, "No support for HW time stamping\n");
..	..	@@ -643,7 +628,8 @@
643	628	config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
644	629	ptp_v2 = PTP_TCR_TSVER2ENA;
645	630	snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
646		- ts_event_en = PTP_TCR_TSEVNTENA;
	631	+ if (priv->synopsys_id < DWMAC_CORE_4_10)
	632	+ ts_event_en = PTP_TCR_TSEVNTENA;
647	633	ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
648	634	ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
649	635	ptp_over_ethernet = PTP_TCR_TSIPENA;
..	..	@@ -698,41 +684,16 @@
698	684	priv->hwts_rx_en = ((config.rx_filter == HWTSTAMP_FILTER_NONE) ? 0 : 1);
699	685	priv->hwts_tx_en = config.tx_type == HWTSTAMP_TX_ON;
700	686
701		- if (!priv->hwts_tx_en && !priv->hwts_rx_en)
702		- stmmac_config_hw_tstamping(priv, priv->ptpaddr, 0);
703		- else {
704		- value = (PTP_TCR_TSENA \| PTP_TCR_TSCFUPDT \| PTP_TCR_TSCTRLSSR \|
705		- tstamp_all \| ptp_v2 \| ptp_over_ethernet \|
706		- ptp_over_ipv6_udp \| ptp_over_ipv4_udp \| ts_event_en \|
707		- ts_master_en \| snap_type_sel);
708		- stmmac_config_hw_tstamping(priv, priv->ptpaddr, value);
	687	+ priv->systime_flags = STMMAC_HWTS_ACTIVE;
709	688
710		- /* program Sub Second Increment reg */
711		- stmmac_config_sub_second_increment(priv,
712		- priv->ptpaddr, priv->plat->clk_ptp_rate,
713		- xmac, &sec_inc);
714		- temp = div_u64(1000000000ULL, sec_inc);
715		-
716		- /* Store sub second increment and flags for later use */
717		- priv->sub_second_inc = sec_inc;
718		- priv->systime_flags = value;
719		-
720		- /* calculate default added value:
721		- * formula is :
722		- * addend = (2^32)/freq_div_ratio;
723		- * where, freq_div_ratio = 1e9ns/sec_inc
724		- */
725		- temp = (u64)(temp << 32);
726		- priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate);
727		- stmmac_config_addend(priv, priv->ptpaddr, priv->default_addend);
728		-
729		- /* initialize system time */
730		- ktime_get_real_ts64(&now);
731		-
732		- /* lower 32 bits of tv_sec are safe until y2106 */
733		- stmmac_init_systime(priv, priv->ptpaddr,
734		- (u32)now.tv_sec, now.tv_nsec);
	689	+ if (priv->hwts_tx_en \|\| priv->hwts_rx_en) {
	690	+ priv->systime_flags \|= tstamp_all \| ptp_v2 \|
	691	+ ptp_over_ethernet \| ptp_over_ipv6_udp \|
	692	+ ptp_over_ipv4_udp \| ts_event_en \|
	693	+ ts_master_en \| snap_type_sel;
735	694	}
	695	+
	696	+ stmmac_config_hw_tstamping(priv, priv->ptpaddr, priv->systime_flags);
736	697
737	698	memcpy(&priv->tstamp_config, &config, sizeof(config));
738	699
..	..	@@ -747,7 +708,7 @@
747	708	* a proprietary structure used to pass information to the driver.
748	709	* Description:
749	710	* This function obtain the current hardware timestamping settings
750		- as requested.
	711	+ * as requested.
751	712	*/
752	713	static int stmmac_hwtstamp_get(struct net_device dev, struct ifreq ifr)
753	714	{
..	..	@@ -762,6 +723,57 @@
762	723	}
763	724
764	725	/**
	726	+ * stmmac_init_tstamp_counter - init hardware timestamping counter
	727	+ * @priv: driver private structure
	728	+ * @systime_flags: timestamping flags
	729	+ * Description:
	730	+ * Initialize hardware counter for packet timestamping.
	731	+ * This is valid as long as the interface is open and not suspended.
	732	+ * Will be rerun after resuming from suspend, case in which the timestamping
	733	+ * flags updated by stmmac_hwtstamp_set() also need to be restored.
	734	+ */
	735	+int stmmac_init_tstamp_counter(struct stmmac_priv *priv, u32 systime_flags)
	736	+{
	737	+ bool xmac = priv->plat->has_gmac4 \|\| priv->plat->has_xgmac;
	738	+ struct timespec64 now;
	739	+ u32 sec_inc = 0;
	740	+ u64 temp = 0;
	741	+
	742	+ if (!(priv->dma_cap.time_stamp \|\| priv->dma_cap.atime_stamp))
	743	+ return -EOPNOTSUPP;
	744	+
	745	+ stmmac_config_hw_tstamping(priv, priv->ptpaddr, systime_flags);
	746	+ priv->systime_flags = systime_flags;
	747	+
	748	+ /* program Sub Second Increment reg */
	749	+ stmmac_config_sub_second_increment(priv, priv->ptpaddr,
	750	+ priv->plat->clk_ptp_rate,
	751	+ xmac, &sec_inc);
	752	+ temp = div_u64(1000000000ULL, sec_inc);
	753	+
	754	+ /* Store sub second increment for later use */
	755	+ priv->sub_second_inc = sec_inc;
	756	+
	757	+ /* calculate default added value:
	758	+ * formula is :
	759	+ * addend = (2^32)/freq_div_ratio;
	760	+ * where, freq_div_ratio = 1e9ns/sec_inc
	761	+ */
	762	+ temp = (u64)(temp << 32);
	763	+ priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate);
	764	+ stmmac_config_addend(priv, priv->ptpaddr, priv->default_addend);
	765	+
	766	+ /* initialize system time */
	767	+ ktime_get_real_ts64(&now);
	768	+
	769	+ /* lower 32 bits of tv_sec are safe until y2106 */
	770	+ stmmac_init_systime(priv, priv->ptpaddr, (u32)now.tv_sec, now.tv_nsec);
	771	+
	772	+ return 0;
	773	+}
	774	+EXPORT_SYMBOL_GPL(stmmac_init_tstamp_counter);
	775	+
	776	+/**
765	777	* stmmac_init_ptp - init PTP
766	778	* @priv: driver private structure
767	779	* Description: this is to verify if the HW supports the PTPv1 or PTPv2.
..	..	@@ -771,9 +783,11 @@
771	783	static int stmmac_init_ptp(struct stmmac_priv *priv)
772	784	{
773	785	bool xmac = priv->plat->has_gmac4 \|\| priv->plat->has_xgmac;
	786	+ int ret;
774	787
775		- if (!(priv->dma_cap.time_stamp \|\| priv->dma_cap.atime_stamp))
776		- return -EOPNOTSUPP;
	788	+ ret = stmmac_init_tstamp_counter(priv, STMMAC_HWTS_ACTIVE);
	789	+ if (ret)
	790	+ return ret;
777	791
778	792	priv->adv_ts = 0;
779	793	/* Check if adv_ts can be enabled for dwmac 4.x / xgmac core */
..	..	@@ -793,121 +807,274 @@
793	807	priv->hwts_tx_en = 0;
794	808	priv->hwts_rx_en = 0;
795	809
796		- stmmac_ptp_register(priv);
797		-
798	810	return 0;
799	811	}
800	812
801	813	static void stmmac_release_ptp(struct stmmac_priv *priv)
802	814	{
803		- if (priv->plat->clk_ptp_ref)
804		- clk_disable_unprepare(priv->plat->clk_ptp_ref);
	815	+ clk_disable_unprepare(priv->plat->clk_ptp_ref);
805	816	stmmac_ptp_unregister(priv);
806	817	}
807	818
808	819	/**
809	820	* stmmac_mac_flow_ctrl - Configure flow control in all queues
810	821	* @priv: driver private structure
	822	+ * @duplex: duplex passed to the next function
811	823	* Description: It is used for configuring the flow control in all queues
812	824	*/
813	825	static void stmmac_mac_flow_ctrl(struct stmmac_priv *priv, u32 duplex)
814	826	{
815	827	u32 tx_cnt = priv->plat->tx_queues_to_use;
816	828
817		- stmmac_flow_ctrl(priv, priv->hw, duplex, priv->flow_ctrl,
818		- priv->pause, tx_cnt);
	829	+ stmmac_flow_ctrl(priv, priv->hw, duplex, priv->flow_ctrl & priv->plat->flow_ctrl,
	830	+ priv->pause, tx_cnt);
819	831	}
820	832
821		-/**
822		- * stmmac_adjust_link - adjusts the link parameters
823		- * @dev: net device structure
824		- * Description: this is the helper called by the physical abstraction layer
825		- * drivers to communicate the phy link status. According the speed and duplex
826		- * this driver can invoke registered glue-logic as well.
827		- * It also invoke the eee initialization because it could happen when switch
828		- * on different networks (that are eee capable).
829		- */
830		-static void stmmac_adjust_link(struct net_device *dev)
	833	+static void stmmac_validate(struct phylink_config *config,
	834	+ unsigned long *supported,
	835	+ struct phylink_link_state *state)
831	836	{
832		- struct stmmac_priv *priv = netdev_priv(dev);
833		- struct phy_device *phydev = dev->phydev;
834		- bool new_state = false;
	837	+ struct stmmac_priv *priv = netdev_priv(to_net_dev(config->dev));
	838	+ __ETHTOOL_DECLARE_LINK_MODE_MASK(mac_supported) = { 0, };
	839	+ __ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, };
	840	+ int tx_cnt = priv->plat->tx_queues_to_use;
	841	+ int max_speed = priv->plat->max_speed;
835	842
836		- if (!phydev)
837		- return;
	843	+ phylink_set(mac_supported, 10baseT_Half);
	844	+ phylink_set(mac_supported, 10baseT_Full);
	845	+ phylink_set(mac_supported, 100baseT_Half);
	846	+ phylink_set(mac_supported, 100baseT_Full);
	847	+ phylink_set(mac_supported, 1000baseT_Half);
	848	+ phylink_set(mac_supported, 1000baseT_Full);
	849	+ phylink_set(mac_supported, 1000baseKX_Full);
	850	+ phylink_set(mac_supported, 100baseT1_Full);
	851	+ phylink_set(mac_supported, 1000baseT1_Full);
838	852
839		- mutex_lock(&priv->lock);
	853	+ phylink_set(mac_supported, Autoneg);
	854	+ phylink_set(mac_supported, Pause);
	855	+ phylink_set(mac_supported, Asym_Pause);
	856	+ phylink_set_port_modes(mac_supported);
840	857
841		- if (phydev->link) {
842		- u32 ctrl = readl(priv->ioaddr + MAC_CTRL_REG);
843		-
844		- /* Now we make sure that we can be in full duplex mode.
845		- * If not, we operate in half-duplex mode. */
846		- if (phydev->duplex != priv->oldduplex) {
847		- new_state = true;
848		- if (!phydev->duplex)
849		- ctrl &= ~priv->hw->link.duplex;
850		- else
851		- ctrl \|= priv->hw->link.duplex;
852		- priv->oldduplex = phydev->duplex;
	858	+ /* Cut down 1G if asked to */
	859	+ if ((max_speed > 0) && (max_speed < 1000)) {
	860	+ phylink_set(mask, 1000baseT_Full);
	861	+ phylink_set(mask, 1000baseX_Full);
	862	+ } else if (priv->plat->has_xgmac) {
	863	+ if (!max_speed \|\| (max_speed >= 2500)) {
	864	+ phylink_set(mac_supported, 2500baseT_Full);
	865	+ phylink_set(mac_supported, 2500baseX_Full);
853	866	}
854		- /* Flow Control operation */
855		- if (phydev->pause)
856		- stmmac_mac_flow_ctrl(priv, phydev->duplex);
857		-
858		- if (phydev->speed != priv->speed) {
859		- new_state = true;
860		- ctrl &= ~priv->hw->link.speed_mask;
861		- switch (phydev->speed) {
862		- case SPEED_1000:
863		- ctrl \|= priv->hw->link.speed1000;
864		- break;
865		- case SPEED_100:
866		- ctrl \|= priv->hw->link.speed100;
867		- break;
868		- case SPEED_10:
869		- ctrl \|= priv->hw->link.speed10;
870		- break;
871		- default:
872		- netif_warn(priv, link, priv->dev,
873		- "broken speed: %d\n", phydev->speed);
874		- phydev->speed = SPEED_UNKNOWN;
875		- break;
876		- }
877		- if (phydev->speed != SPEED_UNKNOWN)
878		- stmmac_hw_fix_mac_speed(priv);
879		- priv->speed = phydev->speed;
	867	+ if (!max_speed \|\| (max_speed >= 5000)) {
	868	+ phylink_set(mac_supported, 5000baseT_Full);
880	869	}
881		-
882		- writel(ctrl, priv->ioaddr + MAC_CTRL_REG);
883		-
884		- if (!priv->oldlink) {
885		- new_state = true;
886		- priv->oldlink = true;
	870	+ if (!max_speed \|\| (max_speed >= 10000)) {
	871	+ phylink_set(mac_supported, 10000baseSR_Full);
	872	+ phylink_set(mac_supported, 10000baseLR_Full);
	873	+ phylink_set(mac_supported, 10000baseER_Full);
	874	+ phylink_set(mac_supported, 10000baseLRM_Full);
	875	+ phylink_set(mac_supported, 10000baseT_Full);
	876	+ phylink_set(mac_supported, 10000baseKX4_Full);
	877	+ phylink_set(mac_supported, 10000baseKR_Full);
887	878	}
888		- } else if (priv->oldlink) {
889		- new_state = true;
890		- priv->oldlink = false;
891		- priv->speed = SPEED_UNKNOWN;
892		- priv->oldduplex = DUPLEX_UNKNOWN;
	879	+ if (!max_speed \|\| (max_speed >= 25000)) {
	880	+ phylink_set(mac_supported, 25000baseCR_Full);
	881	+ phylink_set(mac_supported, 25000baseKR_Full);
	882	+ phylink_set(mac_supported, 25000baseSR_Full);
	883	+ }
	884	+ if (!max_speed \|\| (max_speed >= 40000)) {
	885	+ phylink_set(mac_supported, 40000baseKR4_Full);
	886	+ phylink_set(mac_supported, 40000baseCR4_Full);
	887	+ phylink_set(mac_supported, 40000baseSR4_Full);
	888	+ phylink_set(mac_supported, 40000baseLR4_Full);
	889	+ }
	890	+ if (!max_speed \|\| (max_speed >= 50000)) {
	891	+ phylink_set(mac_supported, 50000baseCR2_Full);
	892	+ phylink_set(mac_supported, 50000baseKR2_Full);
	893	+ phylink_set(mac_supported, 50000baseSR2_Full);
	894	+ phylink_set(mac_supported, 50000baseKR_Full);
	895	+ phylink_set(mac_supported, 50000baseSR_Full);
	896	+ phylink_set(mac_supported, 50000baseCR_Full);
	897	+ phylink_set(mac_supported, 50000baseLR_ER_FR_Full);
	898	+ phylink_set(mac_supported, 50000baseDR_Full);
	899	+ }
	900	+ if (!max_speed \|\| (max_speed >= 100000)) {
	901	+ phylink_set(mac_supported, 100000baseKR4_Full);
	902	+ phylink_set(mac_supported, 100000baseSR4_Full);
	903	+ phylink_set(mac_supported, 100000baseCR4_Full);
	904	+ phylink_set(mac_supported, 100000baseLR4_ER4_Full);
	905	+ phylink_set(mac_supported, 100000baseKR2_Full);
	906	+ phylink_set(mac_supported, 100000baseSR2_Full);
	907	+ phylink_set(mac_supported, 100000baseCR2_Full);
	908	+ phylink_set(mac_supported, 100000baseLR2_ER2_FR2_Full);
	909	+ phylink_set(mac_supported, 100000baseDR2_Full);
	910	+ }
893	911	}
894	912
895		- if (new_state && netif_msg_link(priv))
896		- phy_print_status(phydev);
	913	+ /* Half-Duplex can only work with single queue */
	914	+ if (tx_cnt > 1) {
	915	+ phylink_set(mask, 10baseT_Half);
	916	+ phylink_set(mask, 100baseT_Half);
	917	+ phylink_set(mask, 1000baseT_Half);
	918	+ }
897	919
898		- mutex_unlock(&priv->lock);
	920	+ linkmode_and(supported, supported, mac_supported);
	921	+ linkmode_andnot(supported, supported, mask);
899	922
900		- if (phydev->is_pseudo_fixed_link)
901		- /* Stop PHY layer to call the hook to adjust the link in case
902		- * of a switch is attached to the stmmac driver.
903		- */
904		- phydev->irq = PHY_IGNORE_INTERRUPT;
905		- else
906		- /* At this stage, init the EEE if supported.
907		- * Never called in case of fixed_link.
908		- */
909		- priv->eee_enabled = stmmac_eee_init(priv);
	923	+ linkmode_and(state->advertising, state->advertising, mac_supported);
	924	+ linkmode_andnot(state->advertising, state->advertising, mask);
	925	+
	926	+ /* If PCS is supported, check which modes it supports. */
	927	+ stmmac_xpcs_validate(priv, &priv->hw->xpcs_args, supported, state);
910	928	}
	929	+
	930	+static void stmmac_mac_pcs_get_state(struct phylink_config *config,
	931	+ struct phylink_link_state *state)
	932	+{
	933	+ struct stmmac_priv *priv = netdev_priv(to_net_dev(config->dev));
	934	+
	935	+ state->link = 0;
	936	+ stmmac_xpcs_get_state(priv, &priv->hw->xpcs_args, state);
	937	+}
	938	+
	939	+static void stmmac_mac_config(struct phylink_config *config, unsigned int mode,
	940	+ const struct phylink_link_state *state)
	941	+{
	942	+ struct stmmac_priv *priv = netdev_priv(to_net_dev(config->dev));
	943	+
	944	+ stmmac_xpcs_config(priv, &priv->hw->xpcs_args, state);
	945	+}
	946	+
	947	+static void stmmac_mac_an_restart(struct phylink_config *config)
	948	+{
	949	+ /* Not Supported */
	950	+}
	951	+
	952	+static void stmmac_mac_link_down(struct phylink_config *config,
	953	+ unsigned int mode, phy_interface_t interface)
	954	+{
	955	+ struct stmmac_priv *priv = netdev_priv(to_net_dev(config->dev));
	956	+
	957	+ stmmac_mac_set(priv, priv->ioaddr, false);
	958	+ priv->eee_active = false;
	959	+ priv->tx_lpi_enabled = false;
	960	+ stmmac_eee_init(priv);
	961	+ stmmac_set_eee_pls(priv, priv->hw, false);
	962	+}
	963	+
	964	+static void stmmac_mac_link_up(struct phylink_config *config,
	965	+ struct phy_device *phy,
	966	+ unsigned int mode, phy_interface_t interface,
	967	+ int speed, int duplex,
	968	+ bool tx_pause, bool rx_pause)
	969	+{
	970	+ struct stmmac_priv *priv = netdev_priv(to_net_dev(config->dev));
	971	+ u32 ctrl;
	972	+
	973	+ stmmac_xpcs_link_up(priv, &priv->hw->xpcs_args, speed, interface);
	974	+
	975	+ ctrl = readl(priv->ioaddr + MAC_CTRL_REG);
	976	+ ctrl &= ~priv->hw->link.speed_mask;
	977	+
	978	+ if (interface == PHY_INTERFACE_MODE_USXGMII) {
	979	+ switch (speed) {
	980	+ case SPEED_10000:
	981	+ ctrl \|= priv->hw->link.xgmii.speed10000;
	982	+ break;
	983	+ case SPEED_5000:
	984	+ ctrl \|= priv->hw->link.xgmii.speed5000;
	985	+ break;
	986	+ case SPEED_2500:
	987	+ ctrl \|= priv->hw->link.xgmii.speed2500;
	988	+ break;
	989	+ default:
	990	+ return;
	991	+ }
	992	+ } else if (interface == PHY_INTERFACE_MODE_XLGMII) {
	993	+ switch (speed) {
	994	+ case SPEED_100000:
	995	+ ctrl \|= priv->hw->link.xlgmii.speed100000;
	996	+ break;
	997	+ case SPEED_50000:
	998	+ ctrl \|= priv->hw->link.xlgmii.speed50000;
	999	+ break;
	1000	+ case SPEED_40000:
	1001	+ ctrl \|= priv->hw->link.xlgmii.speed40000;
	1002	+ break;
	1003	+ case SPEED_25000:
	1004	+ ctrl \|= priv->hw->link.xlgmii.speed25000;
	1005	+ break;
	1006	+ case SPEED_10000:
	1007	+ ctrl \|= priv->hw->link.xgmii.speed10000;
	1008	+ break;
	1009	+ case SPEED_2500:
	1010	+ ctrl \|= priv->hw->link.speed2500;
	1011	+ break;
	1012	+ case SPEED_1000:
	1013	+ ctrl \|= priv->hw->link.speed1000;
	1014	+ break;
	1015	+ default:
	1016	+ return;
	1017	+ }
	1018	+ } else {
	1019	+ switch (speed) {
	1020	+ case SPEED_2500:
	1021	+ ctrl \|= priv->hw->link.speed2500;
	1022	+ break;
	1023	+ case SPEED_1000:
	1024	+ ctrl \|= priv->hw->link.speed1000;
	1025	+ break;
	1026	+ case SPEED_100:
	1027	+ ctrl \|= priv->hw->link.speed100;
	1028	+ break;
	1029	+ case SPEED_10:
	1030	+ ctrl \|= priv->hw->link.speed10;
	1031	+ break;
	1032	+ default:
	1033	+ return;
	1034	+ }
	1035	+ }
	1036	+
	1037	+ priv->speed = speed;
	1038	+
	1039	+ if (priv->plat->fix_mac_speed)
	1040	+ priv->plat->fix_mac_speed(priv->plat->bsp_priv, speed);
	1041	+
	1042	+ if (!duplex)
	1043	+ ctrl &= ~priv->hw->link.duplex;
	1044	+ else
	1045	+ ctrl \|= priv->hw->link.duplex;
	1046	+
	1047	+ /* Flow Control operation */
	1048	+ if (rx_pause && tx_pause)
	1049	+ priv->flow_ctrl = FLOW_AUTO;
	1050	+ else if (rx_pause && !tx_pause)
	1051	+ priv->flow_ctrl = FLOW_RX;
	1052	+ else if (!rx_pause && tx_pause)
	1053	+ priv->flow_ctrl = FLOW_TX;
	1054	+ else
	1055	+ priv->flow_ctrl = FLOW_OFF;
	1056	+
	1057	+ stmmac_mac_flow_ctrl(priv, duplex);
	1058	+
	1059	+ writel(ctrl, priv->ioaddr + MAC_CTRL_REG);
	1060	+
	1061	+ stmmac_mac_set(priv, priv->ioaddr, true);
	1062	+ if (phy && priv->dma_cap.eee) {
	1063	+ priv->eee_active = phy_init_eee(phy, 1) >= 0;
	1064	+ priv->eee_enabled = stmmac_eee_init(priv);
	1065	+ priv->tx_lpi_enabled = priv->eee_enabled;
	1066	+ stmmac_set_eee_pls(priv, priv->hw, true);
	1067	+ }
	1068	+}
	1069	+
	1070	+static const struct phylink_mac_ops stmmac_phylink_mac_ops = {
	1071	+ .validate = stmmac_validate,
	1072	+ .mac_pcs_get_state = stmmac_mac_pcs_get_state,
	1073	+ .mac_config = stmmac_mac_config,
	1074	+ .mac_an_restart = stmmac_mac_an_restart,
	1075	+ .mac_link_down = stmmac_mac_link_down,
	1076	+ .mac_link_up = stmmac_mac_link_up,
	1077	+};
911	1078
912	1079	/**
913	1080	* stmmac_check_pcs_mode - verify if RGMII/SGMII is supported
..	..	@@ -934,6 +1101,7 @@
934	1101	}
935	1102	}
936	1103
	1104	+#if 0
937	1105	static void rtl8211F_led_control(struct phy_device *phydev)
938	1106	{
939	1107	printk("ben debug:rtl8211F_led_control...1 \n");
..	..	@@ -950,6 +1118,44 @@
950	1118	// phy_write(phydev, 16, 0x6C0A);
951	1119	printk("ben debug:rtl8211F_led_control...2 \n");
952	1120	}
	1121	+#endif
	1122	+#define RTL_8211F_PHY_ID 0x001cc916
	1123	+#define RTL_8211F_PHY_ID_MASK 0x001fffff
	1124	+#define RTL_8211F_PAGE_SELECT 0x1f
	1125	+#define RTL_8211F_LCR_ADDR 0x10
	1126	+
	1127	+#define GREEN_LED 0 // ÂÌµÆÊÇLED0
	1128	+#define YELLOW0_LED 1 // »ÆµÆÊÇLED1
	1129	+#define YELLOW1_LED 2 // »ÆµÆÊÇLED2
	1130	+
	1131	+static int rtl8211F_led_control(struct phy_device *phydev)
	1132	+{
	1133	+ unsigned int temp;
	1134	+
	1135	+ printk("<<<<<<ben test led ctrl start... %s\n",__FUNCTION__);
	1136	+ if(!phydev) return 0;
	1137	+ if(phydev->phy_id!=0x001cc916) return 0; /* only for 8211E*/
	1138	+
	1139	+ // ÅäÖÃÍø¿ÚLEDµÆ
	1140	+ phy_write(phydev, 31, 0xd04);
	1141	+ temp = 0x02 << (5 * GREEN_LED); // ÂÌµÆ±íÊ¾Á¬½Ó×´Ì¬
	1142	+ temp \|= 0x08 << (5 * YELLOW0_LED); // »ÆµÆ±íÊ¾Á¬½Ó×´Ì¬
	1143	+
	1144	+ temp \|= 0x1b << (5 * YELLOW1_LED); // »ÆµÆ±íÊ¾Êý¾Ý°üÊÕ·¢×´Ì¬
	1145	+ phy_write(phydev, 0x10, temp);
	1146	+
	1147	+ temp = 1 << (YELLOW1_LED + 1); // ÂÌµÆ²»Ö¸Ê¾EEE½ÚÄÜ×´Ì¬, ²åÁËÍøÏßºóµÆÒª³£ÁÁ
	1148	+ // µ«»ÆµÆÒªÖ¸Ê¾EEE½ÚÄÜ×´Ì¬, Ã»ÓÐÊý¾Ý°üÊ±µÆÒªÏ¨Ãð
	1149	+ phy_write(phydev, 0x11, 0x00);
	1150	+ phy_write(phydev, 31, 0);
	1151	+
	1152	+ // ²åÉÏÍøÏßºó, ÂÌµÆ³£ÁÁ; °ÎµôÍøÏßºó, ÂÌµÆÏ¨Ãð
	1153	+ // Á´Â·Ö§³ÖEEE½ÚÄÜ: ÓÐÊý¾Ý´«Êä, »ÆµÆÉÁË¸, ·ñÔòÏ¨Ãð
	1154	+ // Á´Â·²»Ö§³ÖEEE½ÚÄÜ: ÓÐÊý¾Ý´«Êä, »ÆµÆÉÁË¸, ·ñÔò³£ÁÁ
	1155	+
	1156	+ printk("<<<<<<<ben test led ctrl end %s\n",__FUNCTION__);
	1157	+ return 0;
	1158	+}
953	1159
954	1160	/**
955	1161	* stmmac_init_phy - PHY initialization
..	..	@@ -962,85 +1168,83 @@
962	1168	static int stmmac_init_phy(struct net_device *dev)
963	1169	{
964	1170	struct stmmac_priv *priv = netdev_priv(dev);
965		- u32 tx_cnt = priv->plat->tx_queues_to_use;
966		- struct phy_device *phydev;
967		- char phy_id_fmt[MII_BUS_ID_SIZE + 3];
968		- char bus_id[MII_BUS_ID_SIZE];
969		- int interface = priv->plat->interface;
970		- int max_speed = priv->plat->max_speed;
971		- priv->oldlink = false;
972		- priv->speed = SPEED_UNKNOWN;
973		- priv->oldduplex = DUPLEX_UNKNOWN;
	1171	+ struct device_node *node;
	1172	+ int ret;
974	1173
975		- if (priv->plat->phy_node) {
976		- phydev = of_phy_connect(dev, priv->plat->phy_node,
977		- &stmmac_adjust_link, 0, interface);
978		- } else {
979		- snprintf(bus_id, MII_BUS_ID_SIZE, "stmmac-%x",
980		- priv->plat->bus_id);
981	1174
982		- snprintf(phy_id_fmt, MII_BUS_ID_SIZE + 3, PHY_ID_FMT, bus_id,
983		- priv->plat->phy_addr);
984		- netdev_dbg(priv->dev, "%s: trying to attach to %s\n", __func__,
985		- phy_id_fmt);
	1175	+ printk("ben stmmac_init_phy .. \n");
	1176	+ mdelay(2000);
	1177	+ printk("ben stmmac_init_phy delay .. \n");
	1178	+ if (priv->plat->integrated_phy_power)
	1179	+ ret = priv->plat->integrated_phy_power(priv->plat->bsp_priv, true);
986	1180
987		- phydev = phy_connect(dev, phy_id_fmt, &stmmac_adjust_link,
988		- interface);
989		- }
	1181	+ node = priv->plat->phylink_node;
990	1182
991		- if (IS_ERR_OR_NULL(phydev)) {
992		- netdev_err(priv->dev, "Could not attach to PHY\n");
993		- if (!phydev)
	1183	+ if (node)
	1184	+ {
	1185	+ //printk("ben ttt.. \n");
	1186	+ ret = phylink_of_phy_connect(priv->phylink, node, 0);
	1187	+ //printk("ben ttt:%d \n", ret);
	1188	+ }
	1189	+
	1190	+ /* Some DT bindings do not set-up the PHY handle. Let's try to
	1191	+ * manually parse it
	1192	+ */
	1193	+ //printk("ben:stmmac_init_phy..1 \n");
	1194	+ if (!node \|\| ret) {
	1195	+ //if (1) {
	1196	+ int addr = priv->plat->phy_addr;
	1197	+ struct phy_device *phydev;
	1198	+
	1199	+ //printk("ben:stmmac_init_phy..2 \n");
	1200	+ phydev = mdiobus_get_phy(priv->mii, addr);
	1201	+ if (!phydev) {
	1202	+ netdev_err(priv->dev, "no phy at addr %d\n", addr);
994	1203	return -ENODEV;
	1204	+ }
995	1205
996		- return PTR_ERR(phydev);
	1206	+ //rtl8211F_led_control(phydev);
	1207	+
	1208	+ //printk("ben:stmmac_init_phy..3 \n");
	1209	+ ret = phylink_connect_phy(priv->phylink, phydev);
	1210	+ //rtl8211F_led_control(phydev);
997	1211	}
998	1212
999		- /* Stop Advertising 1000BASE Capability if interface is not GMII */
1000		- if ((interface == PHY_INTERFACE_MODE_MII) \|\|
1001		- (interface == PHY_INTERFACE_MODE_RMII) \|\|
1002		- (max_speed < 1000 && max_speed > 0))
1003		- phydev->advertising &= ~(SUPPORTED_1000baseT_Half \|
1004		- SUPPORTED_1000baseT_Full);
	1213	+ if (!priv->plat->pmt) {
	1214	+ struct ethtool_wolinfo wol = { .cmd = ETHTOOL_GWOL };
1005	1215
1006		- /*
1007		- * Half-duplex mode not supported with multiqueue
1008		- * half-duplex can only works with single queue
1009		- */
1010		- if (tx_cnt > 1)
1011		- phydev->supported &= ~(SUPPORTED_1000baseT_Half \|
1012		- SUPPORTED_100baseT_Half \|
1013		- SUPPORTED_10baseT_Half);
1014		-
1015		- /*
1016		- * Broken HW is sometimes missing the pull-up resistor on the
1017		- * MDIO line, which results in reads to non-existent devices returning
1018		- * 0 rather than 0xffff. Catch this here and treat 0 as a non-existent
1019		- * device as well.
1020		- * Note: phydev->phy_id is the result of reading the UID PHY registers.
1021		- */
1022		- if (!priv->plat->phy_node && phydev->phy_id == 0) {
1023		- phy_disconnect(phydev);
1024		- return -ENODEV;
	1216	+ phylink_ethtool_get_wol(priv->phylink, &wol);
	1217	+ device_set_wakeup_capable(priv->device, !!wol.supported);
1025	1218	}
	1219	+ return ret;
	1220	+}
1026	1221
1027		- /* stmmac_adjust_link will change this to PHY_IGNORE_INTERRUPT to avoid
1028		- * subsequent PHY polling, make sure we force a link transition if
1029		- * we have a UP/DOWN/UP transition
1030		- */
1031		- if (phydev->is_pseudo_fixed_link)
1032		- phydev->irq = PHY_POLL;
	1222	+static int stmmac_phy_setup(struct stmmac_priv *priv)
	1223	+{
	1224	+ struct fwnode_handle *fwnode = of_fwnode_handle(priv->plat->phylink_node);
	1225	+ int mode = priv->plat->phy_interface;
	1226	+ struct phylink *phylink;
1033	1227
1034		- phy_attached_info(phydev);
	1228	+ priv->phylink_config.dev = &priv->dev->dev;
	1229	+ priv->phylink_config.type = PHYLINK_NETDEV;
	1230	+ priv->phylink_config.pcs_poll = true;
1035	1231
1036		- //add ben
1037		- rtl8211F_led_control(phydev);
	1232	+ if (!fwnode)
	1233	+ fwnode = dev_fwnode(priv->device);
	1234	+
	1235	+ phylink = phylink_create(&priv->phylink_config, fwnode,
	1236	+ mode, &stmmac_phylink_mac_ops);
	1237	+ if (IS_ERR(phylink))
	1238	+ return PTR_ERR(phylink);
	1239	+
	1240	+ priv->phylink = phylink;
1038	1241	return 0;
1039	1242	}
1040	1243
1041	1244	static void stmmac_display_rx_rings(struct stmmac_priv *priv)
1042	1245	{
1043	1246	u32 rx_cnt = priv->plat->rx_queues_to_use;
	1247	+ unsigned int desc_size;
1044	1248	void *head_rx;
1045	1249	u32 queue;
1046	1250
..	..	@@ -1050,19 +1254,24 @@
1050	1254
1051	1255	pr_info("\tRX Queue %u rings\n", queue);
1052	1256
1053		- if (priv->extend_desc)
	1257	+ if (priv->extend_desc) {
1054	1258	head_rx = (void *)rx_q->dma_erx;
1055		- else
	1259	+ desc_size = sizeof(struct dma_extended_desc);
	1260	+ } else {
1056	1261	head_rx = (void *)rx_q->dma_rx;
	1262	+ desc_size = sizeof(struct dma_desc);
	1263	+ }
1057	1264
1058	1265	/* Display RX ring */
1059		- stmmac_display_ring(priv, head_rx, DMA_RX_SIZE, true);
	1266	+ stmmac_display_ring(priv, head_rx, priv->dma_rx_size, true,
	1267	+ rx_q->dma_rx_phy, desc_size);
1060	1268	}
1061	1269	}
1062	1270
1063	1271	static void stmmac_display_tx_rings(struct stmmac_priv *priv)
1064	1272	{
1065	1273	u32 tx_cnt = priv->plat->tx_queues_to_use;
	1274	+ unsigned int desc_size;
1066	1275	void *head_tx;
1067	1276	u32 queue;
1068	1277
..	..	@@ -1072,12 +1281,19 @@
1072	1281
1073	1282	pr_info("\tTX Queue %d rings\n", queue);
1074	1283
1075		- if (priv->extend_desc)
	1284	+ if (priv->extend_desc) {
1076	1285	head_tx = (void *)tx_q->dma_etx;
1077		- else
	1286	+ desc_size = sizeof(struct dma_extended_desc);
	1287	+ } else if (tx_q->tbs & STMMAC_TBS_AVAIL) {
	1288	+ head_tx = (void *)tx_q->dma_entx;
	1289	+ desc_size = sizeof(struct dma_edesc);
	1290	+ } else {
1078	1291	head_tx = (void *)tx_q->dma_tx;
	1292	+ desc_size = sizeof(struct dma_desc);
	1293	+ }
1079	1294
1080		- stmmac_display_ring(priv, head_tx, DMA_TX_SIZE, false);
	1295	+ stmmac_display_ring(priv, head_tx, priv->dma_tx_size, false,
	1296	+ tx_q->dma_tx_phy, desc_size);
1081	1297	}
1082	1298	}
1083	1299
..	..	@@ -1121,16 +1337,16 @@
1121	1337	int i;
1122	1338
1123	1339	/* Clear the RX descriptors */
1124		- for (i = 0; i < DMA_RX_SIZE; i++)
	1340	+ for (i = 0; i < priv->dma_rx_size; i++)
1125	1341	if (priv->extend_desc)
1126	1342	stmmac_init_rx_desc(priv, &rx_q->dma_erx[i].basic,
1127	1343	priv->use_riwt, priv->mode,
1128		- (i == DMA_RX_SIZE - 1),
	1344	+ (i == priv->dma_rx_size - 1),
1129	1345	priv->dma_buf_sz);
1130	1346	else
1131	1347	stmmac_init_rx_desc(priv, &rx_q->dma_rx[i],
1132	1348	priv->use_riwt, priv->mode,
1133		- (i == DMA_RX_SIZE - 1),
	1349	+ (i == priv->dma_rx_size - 1),
1134	1350	priv->dma_buf_sz);
1135	1351	}
1136	1352
..	..	@@ -1147,13 +1363,19 @@
1147	1363	int i;
1148	1364
1149	1365	/* Clear the TX descriptors */
1150		- for (i = 0; i < DMA_TX_SIZE; i++)
	1366	+ for (i = 0; i < priv->dma_tx_size; i++) {
	1367	+ int last = (i == (priv->dma_tx_size - 1));
	1368	+ struct dma_desc *p;
	1369	+
1151	1370	if (priv->extend_desc)
1152		- stmmac_init_tx_desc(priv, &tx_q->dma_etx[i].basic,
1153		- priv->mode, (i == DMA_TX_SIZE - 1));
	1371	+ p = &tx_q->dma_etx[i].basic;
	1372	+ else if (tx_q->tbs & STMMAC_TBS_AVAIL)
	1373	+ p = &tx_q->dma_entx[i].basic;
1154	1374	else
1155		- stmmac_init_tx_desc(priv, &tx_q->dma_tx[i],
1156		- priv->mode, (i == DMA_TX_SIZE - 1));
	1375	+ p = &tx_q->dma_tx[i];
	1376	+
	1377	+ stmmac_init_tx_desc(priv, p, priv->mode, last);
	1378	+ }
1157	1379	}
1158	1380
1159	1381	/**
..	..	@@ -1191,26 +1413,30 @@
1191	1413	int i, gfp_t flags, u32 queue)
1192	1414	{
1193	1415	struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
1194		- struct sk_buff *skb;
	1416	+ struct stmmac_rx_buffer *buf = &rx_q->buf_pool[i];
	1417	+ gfp_t gfp = (GFP_ATOMIC \| __GFP_NOWARN);
1195	1418
1196		- skb = __netdev_alloc_skb_ip_align(priv->dev, priv->dma_buf_sz, flags);
1197		- if (!skb) {
1198		- netdev_err(priv->dev,
1199		- "%s: Rx init fails; skb is NULL\n", __func__);
	1419	+ if (priv->dma_cap.addr64 <= 32)
	1420	+ gfp \|= GFP_DMA32;
	1421	+
	1422	+ buf->page = page_pool_alloc_pages(rx_q->page_pool, gfp);
	1423	+ if (!buf->page)
1200	1424	return -ENOMEM;
1201		- }
1202		- rx_q->rx_skbuff[i] = skb;
1203		- rx_q->rx_skbuff_dma[i] = dma_map_single(priv->device, skb->data,
1204		- priv->dma_buf_sz,
1205		- DMA_FROM_DEVICE);
1206		- if (dma_mapping_error(priv->device, rx_q->rx_skbuff_dma[i])) {
1207		- netdev_err(priv->dev, "%s: DMA mapping error\n", __func__);
1208		- dev_kfree_skb_any(skb);
1209		- return -EINVAL;
	1425	+
	1426	+ if (priv->sph) {
	1427	+ buf->sec_page = page_pool_alloc_pages(rx_q->page_pool, gfp);
	1428	+ if (!buf->sec_page)
	1429	+ return -ENOMEM;
	1430	+
	1431	+ buf->sec_addr = page_pool_get_dma_addr(buf->sec_page);
	1432	+ stmmac_set_desc_sec_addr(priv, p, buf->sec_addr, true);
	1433	+ } else {
	1434	+ buf->sec_page = NULL;
	1435	+ stmmac_set_desc_sec_addr(priv, p, buf->sec_addr, false);
1210	1436	}
1211	1437
1212		- stmmac_set_desc_addr(priv, p, rx_q->rx_skbuff_dma[i]);
1213		-
	1438	+ buf->addr = page_pool_get_dma_addr(buf->page);
	1439	+ stmmac_set_desc_addr(priv, p, buf->addr);
1214	1440	if (priv->dma_buf_sz == BUF_SIZE_16KiB)
1215	1441	stmmac_init_desc3(priv, p);
1216	1442
..	..	@@ -1226,13 +1452,15 @@
1226	1452	static void stmmac_free_rx_buffer(struct stmmac_priv *priv, u32 queue, int i)
1227	1453	{
1228	1454	struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
	1455	+ struct stmmac_rx_buffer *buf = &rx_q->buf_pool[i];
1229	1456
1230		- if (rx_q->rx_skbuff[i]) {
1231		- dma_unmap_single(priv->device, rx_q->rx_skbuff_dma[i],
1232		- priv->dma_buf_sz, DMA_FROM_DEVICE);
1233		- dev_kfree_skb_any(rx_q->rx_skbuff[i]);
1234		- }
1235		- rx_q->rx_skbuff[i] = NULL;
	1457	+ if (buf->page)
	1458	+ page_pool_put_full_page(rx_q->page_pool, buf->page, false);
	1459	+ buf->page = NULL;
	1460	+
	1461	+ if (buf->sec_page)
	1462	+ page_pool_put_full_page(rx_q->page_pool, buf->sec_page, false);
	1463	+ buf->sec_page = NULL;
1236	1464	}
1237	1465
1238	1466	/**
..	..	@@ -1279,18 +1507,8 @@
1279	1507	struct stmmac_priv *priv = netdev_priv(dev);
1280	1508	u32 rx_count = priv->plat->rx_queues_to_use;
1281	1509	int ret = -ENOMEM;
1282		- int bfsize = 0;
1283	1510	int queue;
1284	1511	int i;
1285		-
1286		- bfsize = stmmac_set_16kib_bfsize(priv, dev->mtu);
1287		- if (bfsize < 0)
1288		- bfsize = 0;
1289		-
1290		- if (bfsize < BUF_SIZE_16KiB)
1291		- bfsize = stmmac_set_bfsize(dev->mtu, priv->dma_buf_sz);
1292		-
1293		- priv->dma_buf_sz = bfsize;
1294	1512
1295	1513	/* RX INITIALIZATION */
1296	1514	netif_dbg(priv, probe, priv->dev,
..	..	@@ -1303,7 +1521,9 @@
1303	1521	"(%s) dma_rx_phy=0x%08x\n", __func__,
1304	1522	(u32)rx_q->dma_rx_phy);
1305	1523
1306		- for (i = 0; i < DMA_RX_SIZE; i++) {
	1524	+ stmmac_clear_rx_descriptors(priv, queue);
	1525	+
	1526	+ for (i = 0; i < priv->dma_rx_size; i++) {
1307	1527	struct dma_desc *p;
1308	1528
1309	1529	if (priv->extend_desc)
..	..	@@ -1315,29 +1535,23 @@
1315	1535	queue);
1316	1536	if (ret)
1317	1537	goto err_init_rx_buffers;
1318		-
1319		- netif_dbg(priv, probe, priv->dev, "[%p]\t[%p]\t[%x]\n",
1320		- rx_q->rx_skbuff[i], rx_q->rx_skbuff[i]->data,
1321		- (unsigned int)rx_q->rx_skbuff_dma[i]);
1322	1538	}
1323	1539
1324	1540	rx_q->cur_rx = 0;
1325		- rx_q->dirty_rx = (unsigned int)(i - DMA_RX_SIZE);
1326		-
1327		- stmmac_clear_rx_descriptors(priv, queue);
	1541	+ rx_q->dirty_rx = (unsigned int)(i - priv->dma_rx_size);
1328	1542
1329	1543	/* Setup the chained descriptor addresses */
1330	1544	if (priv->mode == STMMAC_CHAIN_MODE) {
1331	1545	if (priv->extend_desc)
1332	1546	stmmac_mode_init(priv, rx_q->dma_erx,
1333		- rx_q->dma_rx_phy, DMA_RX_SIZE, 1);
	1547	+ rx_q->dma_rx_phy,
	1548	+ priv->dma_rx_size, 1);
1334	1549	else
1335	1550	stmmac_mode_init(priv, rx_q->dma_rx,
1336		- rx_q->dma_rx_phy, DMA_RX_SIZE, 0);
	1551	+ rx_q->dma_rx_phy,
	1552	+ priv->dma_rx_size, 0);
1337	1553	}
1338	1554	}
1339		-
1340		- buf_sz = bfsize;
1341	1555
1342	1556	return 0;
1343	1557
..	..	@@ -1349,7 +1563,7 @@
1349	1563	if (queue == 0)
1350	1564	break;
1351	1565
1352		- i = DMA_RX_SIZE;
	1566	+ i = priv->dma_rx_size;
1353	1567	queue--;
1354	1568	}
1355	1569
..	..	@@ -1381,16 +1595,20 @@
1381	1595	if (priv->mode == STMMAC_CHAIN_MODE) {
1382	1596	if (priv->extend_desc)
1383	1597	stmmac_mode_init(priv, tx_q->dma_etx,
1384		- tx_q->dma_tx_phy, DMA_TX_SIZE, 1);
1385		- else
	1598	+ tx_q->dma_tx_phy,
	1599	+ priv->dma_tx_size, 1);
	1600	+ else if (!(tx_q->tbs & STMMAC_TBS_AVAIL))
1386	1601	stmmac_mode_init(priv, tx_q->dma_tx,
1387		- tx_q->dma_tx_phy, DMA_TX_SIZE, 0);
	1602	+ tx_q->dma_tx_phy,
	1603	+ priv->dma_tx_size, 0);
1388	1604	}
1389	1605
1390		- for (i = 0; i < DMA_TX_SIZE; i++) {
	1606	+ for (i = 0; i < priv->dma_tx_size; i++) {
1391	1607	struct dma_desc *p;
1392	1608	if (priv->extend_desc)
1393	1609	p = &((tx_q->dma_etx + i)->basic);
	1610	+ else if (tx_q->tbs & STMMAC_TBS_AVAIL)
	1611	+ p = &((tx_q->dma_entx + i)->basic);
1394	1612	else
1395	1613	p = tx_q->dma_tx + i;
1396	1614
..	..	@@ -1449,7 +1667,7 @@
1449	1667	{
1450	1668	int i;
1451	1669
1452		- for (i = 0; i < DMA_RX_SIZE; i++)
	1670	+ for (i = 0; i < priv->dma_rx_size; i++)
1453	1671	stmmac_free_rx_buffer(priv, queue, i);
1454	1672	}
1455	1673
..	..	@@ -1462,7 +1680,7 @@
1462	1680	{
1463	1681	int i;
1464	1682
1465		- for (i = 0; i < DMA_TX_SIZE; i++)
	1683	+ for (i = 0; i < priv->dma_tx_size; i++)
1466	1684	stmmac_free_tx_buffer(priv, queue, i);
1467	1685	}
1468	1686
..	..	@@ -1497,16 +1715,17 @@
1497	1715
1498	1716	/* Free DMA regions of consistent memory previously allocated */
1499	1717	if (!priv->extend_desc)
1500		- dma_free_coherent(priv->device,
1501		- DMA_RX_SIZE * sizeof(struct dma_desc),
	1718	+ dma_free_coherent(priv->device, priv->dma_rx_size *
	1719	+ sizeof(struct dma_desc),
1502	1720	rx_q->dma_rx, rx_q->dma_rx_phy);
1503	1721	else
1504		- dma_free_coherent(priv->device, DMA_RX_SIZE *
	1722	+ dma_free_coherent(priv->device, priv->dma_rx_size *
1505	1723	sizeof(struct dma_extended_desc),
1506	1724	rx_q->dma_erx, rx_q->dma_rx_phy);
1507	1725
1508		- kfree(rx_q->rx_skbuff_dma);
1509		- kfree(rx_q->rx_skbuff);
	1726	+ kfree(rx_q->buf_pool);
	1727	+ if (rx_q->page_pool)
	1728	+ page_pool_destroy(rx_q->page_pool);
1510	1729	}
1511	1730	}
1512	1731
..	..	@@ -1522,19 +1741,26 @@
1522	1741	/* Free TX queue resources */
1523	1742	for (queue = 0; queue < tx_count; queue++) {
1524	1743	struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
	1744	+ size_t size;
	1745	+ void *addr;
1525	1746
1526	1747	/* Release the DMA TX socket buffers */
1527	1748	dma_free_tx_skbufs(priv, queue);
1528	1749
1529		- /* Free DMA regions of consistent memory previously allocated */
1530		- if (!priv->extend_desc)
1531		- dma_free_coherent(priv->device,
1532		- DMA_TX_SIZE * sizeof(struct dma_desc),
1533		- tx_q->dma_tx, tx_q->dma_tx_phy);
1534		- else
1535		- dma_free_coherent(priv->device, DMA_TX_SIZE *
1536		- sizeof(struct dma_extended_desc),
1537		- tx_q->dma_etx, tx_q->dma_tx_phy);
	1750	+ if (priv->extend_desc) {
	1751	+ size = sizeof(struct dma_extended_desc);
	1752	+ addr = tx_q->dma_etx;
	1753	+ } else if (tx_q->tbs & STMMAC_TBS_AVAIL) {
	1754	+ size = sizeof(struct dma_edesc);
	1755	+ addr = tx_q->dma_entx;
	1756	+ } else {
	1757	+ size = sizeof(struct dma_desc);
	1758	+ addr = tx_q->dma_tx;
	1759	+ }
	1760	+
	1761	+ size *= priv->dma_tx_size;
	1762	+
	1763	+ dma_free_coherent(priv->device, size, addr, tx_q->dma_tx_phy);
1538	1764
1539	1765	kfree(tx_q->tx_skbuff_dma);
1540	1766	kfree(tx_q->tx_skbuff);
..	..	@@ -1558,39 +1784,49 @@
1558	1784	/* RX queues buffers and DMA */
1559	1785	for (queue = 0; queue < rx_count; queue++) {
1560	1786	struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
	1787	+ struct page_pool_params pp_params = { 0 };
	1788	+ unsigned int num_pages;
1561	1789
1562	1790	rx_q->queue_index = queue;
1563	1791	rx_q->priv_data = priv;
1564	1792
1565		- rx_q->rx_skbuff_dma = kmalloc_array(DMA_RX_SIZE,
1566		- sizeof(dma_addr_t),
1567		- GFP_KERNEL);
1568		- if (!rx_q->rx_skbuff_dma)
1569		- goto err_dma;
	1793	+ pp_params.flags = PP_FLAG_DMA_MAP \| PP_FLAG_DMA_SYNC_DEV;
	1794	+ pp_params.pool_size = priv->dma_rx_size;
	1795	+ num_pages = DIV_ROUND_UP(priv->dma_buf_sz, PAGE_SIZE);
	1796	+ pp_params.order = ilog2(num_pages);
	1797	+ pp_params.nid = dev_to_node(priv->device);
	1798	+ pp_params.dev = priv->device;
	1799	+ pp_params.dma_dir = DMA_FROM_DEVICE;
	1800	+ pp_params.max_len = num_pages * PAGE_SIZE;
1570	1801
1571		- rx_q->rx_skbuff = kmalloc_array(DMA_RX_SIZE,
1572		- sizeof(struct sk_buff *),
1573		- GFP_KERNEL);
1574		- if (!rx_q->rx_skbuff)
	1802	+ rx_q->page_pool = page_pool_create(&pp_params);
	1803	+ if (IS_ERR(rx_q->page_pool)) {
	1804	+ ret = PTR_ERR(rx_q->page_pool);
	1805	+ rx_q->page_pool = NULL;
	1806	+ goto err_dma;
	1807	+ }
	1808	+
	1809	+ rx_q->buf_pool = kcalloc(priv->dma_rx_size,
	1810	+ sizeof(*rx_q->buf_pool),
	1811	+ GFP_KERNEL);
	1812	+ if (!rx_q->buf_pool)
1575	1813	goto err_dma;
1576	1814
1577	1815	if (priv->extend_desc) {
1578		- rx_q->dma_erx = dma_zalloc_coherent(priv->device,
1579		- DMA_RX_SIZE *
1580		- sizeof(struct
1581		- dma_extended_desc),
1582		- &rx_q->dma_rx_phy,
1583		- GFP_KERNEL);
	1816	+ rx_q->dma_erx = dma_alloc_coherent(priv->device,
	1817	+ priv->dma_rx_size *
	1818	+ sizeof(struct dma_extended_desc),
	1819	+ &rx_q->dma_rx_phy,
	1820	+ GFP_KERNEL);
1584	1821	if (!rx_q->dma_erx)
1585	1822	goto err_dma;
1586	1823
1587	1824	} else {
1588		- rx_q->dma_rx = dma_zalloc_coherent(priv->device,
1589		- DMA_RX_SIZE *
1590		- sizeof(struct
1591		- dma_desc),
1592		- &rx_q->dma_rx_phy,
1593		- GFP_KERNEL);
	1825	+ rx_q->dma_rx = dma_alloc_coherent(priv->device,
	1826	+ priv->dma_rx_size *
	1827	+ sizeof(struct dma_desc),
	1828	+ &rx_q->dma_rx_phy,
	1829	+ GFP_KERNEL);
1594	1830	if (!rx_q->dma_rx)
1595	1831	goto err_dma;
1596	1832	}
..	..	@@ -1621,48 +1857,50 @@
1621	1857	/* TX queues buffers and DMA */
1622	1858	for (queue = 0; queue < tx_count; queue++) {
1623	1859	struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
	1860	+ size_t size;
	1861	+ void *addr;
1624	1862
1625	1863	tx_q->queue_index = queue;
1626	1864	tx_q->priv_data = priv;
1627	1865
1628		- tx_q->tx_skbuff_dma = kmalloc_array(DMA_TX_SIZE,
1629		- sizeof(*tx_q->tx_skbuff_dma),
1630		- GFP_KERNEL);
	1866	+ tx_q->tx_skbuff_dma = kcalloc(priv->dma_tx_size,
	1867	+ sizeof(*tx_q->tx_skbuff_dma),
	1868	+ GFP_KERNEL);
1631	1869	if (!tx_q->tx_skbuff_dma)
1632	1870	goto err_dma;
1633	1871
1634		- tx_q->tx_skbuff = kmalloc_array(DMA_TX_SIZE,
1635		- sizeof(struct sk_buff *),
1636		- GFP_KERNEL);
	1872	+ tx_q->tx_skbuff = kcalloc(priv->dma_tx_size,
	1873	+ sizeof(struct sk_buff *),
	1874	+ GFP_KERNEL);
1637	1875	if (!tx_q->tx_skbuff)
1638	1876	goto err_dma;
1639	1877
1640		- if (priv->extend_desc) {
1641		- tx_q->dma_etx = dma_zalloc_coherent(priv->device,
1642		- DMA_TX_SIZE *
1643		- sizeof(struct
1644		- dma_extended_desc),
1645		- &tx_q->dma_tx_phy,
1646		- GFP_KERNEL);
1647		- if (!tx_q->dma_etx)
1648		- goto err_dma;
1649		- } else {
1650		- tx_q->dma_tx = dma_zalloc_coherent(priv->device,
1651		- DMA_TX_SIZE *
1652		- sizeof(struct
1653		- dma_desc),
1654		- &tx_q->dma_tx_phy,
1655		- GFP_KERNEL);
1656		- if (!tx_q->dma_tx)
1657		- goto err_dma;
1658		- }
	1878	+ if (priv->extend_desc)
	1879	+ size = sizeof(struct dma_extended_desc);
	1880	+ else if (tx_q->tbs & STMMAC_TBS_AVAIL)
	1881	+ size = sizeof(struct dma_edesc);
	1882	+ else
	1883	+ size = sizeof(struct dma_desc);
	1884	+
	1885	+ size *= priv->dma_tx_size;
	1886	+
	1887	+ addr = dma_alloc_coherent(priv->device, size,
	1888	+ &tx_q->dma_tx_phy, GFP_KERNEL);
	1889	+ if (!addr)
	1890	+ goto err_dma;
	1891	+
	1892	+ if (priv->extend_desc)
	1893	+ tx_q->dma_etx = addr;
	1894	+ else if (tx_q->tbs & STMMAC_TBS_AVAIL)
	1895	+ tx_q->dma_entx = addr;
	1896	+ else
	1897	+ tx_q->dma_tx = addr;
1659	1898	}
1660	1899
1661	1900	return 0;
1662	1901
1663	1902	err_dma:
1664	1903	free_dma_tx_desc_resources(priv);
1665		-
1666	1904	return ret;
1667	1905	}
1668	1906
..	..	@@ -1873,6 +2111,7 @@
1873	2111	/**
1874	2112	* stmmac_tx_clean - to manage the transmission completion
1875	2113	* @priv: driver private structure
	2114	+ * @budget: napi budget limiting this functions packet handling
1876	2115	* @queue: TX queue index
1877	2116	* Description: it reclaims the transmit resources after transmission completes.
1878	2117	*/
..	..	@@ -1894,6 +2133,8 @@
1894	2133
1895	2134	if (priv->extend_desc)
1896	2135	p = (struct dma_desc *)(tx_q->dma_etx + entry);
	2136	+ else if (tx_q->tbs & STMMAC_TBS_AVAIL)
	2137	+ p = &tx_q->dma_entx[entry].basic;
1897	2138	else
1898	2139	p = tx_q->dma_tx + entry;
1899	2140
..	..	@@ -1952,7 +2193,7 @@
1952	2193
1953	2194	stmmac_release_tx_desc(priv, p, priv->mode);
1954	2195
1955		- entry = STMMAC_GET_ENTRY(entry, DMA_TX_SIZE);
	2196	+ entry = STMMAC_GET_ENTRY(entry, priv->dma_tx_size);
1956	2197	}
1957	2198	tx_q->dirty_tx = entry;
1958	2199
..	..	@@ -1961,7 +2202,7 @@
1961	2202
1962	2203	if (unlikely(netif_tx_queue_stopped(netdev_get_tx_queue(priv->dev,
1963	2204	queue))) &&
1964		- stmmac_tx_avail(priv, queue) > STMMAC_TX_THRESH) {
	2205	+ stmmac_tx_avail(priv, queue) > STMMAC_TX_THRESH(priv)) {
1965	2206
1966	2207	netif_dbg(priv, tx_done, priv->dev,
1967	2208	"%s: restart transmit\n", __func__);
..	..	@@ -1970,8 +2211,12 @@
1970	2211
1971	2212	if ((priv->eee_enabled) && (!priv->tx_path_in_lpi_mode)) {
1972	2213	stmmac_enable_eee_mode(priv);
1973		- mod_timer(&priv->eee_ctrl_timer, STMMAC_LPI_T(eee_timer));
	2214	+ mod_timer(&priv->eee_ctrl_timer, STMMAC_LPI_T(priv->tx_lpi_timer));
1974	2215	}
	2216	+
	2217	+ /* We still have pending packets, let's call for a new scheduling */
	2218	+ if (tx_q->dirty_tx != tx_q->cur_tx)
	2219	+ mod_timer(&tx_q->txtimer, STMMAC_COAL_TIMER(priv->tx_coal_timer));
1975	2220
1976	2221	__netif_tx_unlock_bh(netdev_get_tx_queue(priv->dev, queue));
1977	2222
..	..	@@ -1988,23 +2233,18 @@
1988	2233	static void stmmac_tx_err(struct stmmac_priv *priv, u32 chan)
1989	2234	{
1990	2235	struct stmmac_tx_queue *tx_q = &priv->tx_queue[chan];
1991		- int i;
1992	2236
1993	2237	netif_tx_stop_queue(netdev_get_tx_queue(priv->dev, chan));
1994	2238
1995	2239	stmmac_stop_tx_dma(priv, chan);
1996	2240	dma_free_tx_skbufs(priv, chan);
1997		- for (i = 0; i < DMA_TX_SIZE; i++)
1998		- if (priv->extend_desc)
1999		- stmmac_init_tx_desc(priv, &tx_q->dma_etx[i].basic,
2000		- priv->mode, (i == DMA_TX_SIZE - 1));
2001		- else
2002		- stmmac_init_tx_desc(priv, &tx_q->dma_tx[i],
2003		- priv->mode, (i == DMA_TX_SIZE - 1));
	2241	+ stmmac_clear_tx_descriptors(priv, chan);
2004	2242	tx_q->dirty_tx = 0;
2005	2243	tx_q->cur_tx = 0;
2006	2244	tx_q->mss = 0;
2007	2245	netdev_tx_reset_queue(netdev_get_tx_queue(priv->dev, chan));
	2246	+ stmmac_init_tx_chan(priv, priv->ioaddr, priv->plat->dma_cfg,
	2247	+ tx_q->dma_tx_phy, chan);
2008	2248	stmmac_start_tx_dma(priv, chan);
2009	2249
2010	2250	priv->dev->stats.tx_errors++;
..	..	@@ -2063,23 +2303,24 @@
2063	2303	int status = stmmac_dma_interrupt_status(priv, priv->ioaddr,
2064	2304	&priv->xstats, chan);
2065	2305	struct stmmac_channel *ch = &priv->channel[chan];
2066		- bool needs_work = false;
	2306	+ unsigned long flags;
2067	2307
2068		- if ((status & handle_rx) && ch->has_rx) {
2069		- needs_work = true;
2070		- } else {
2071		- status &= ~handle_rx;
	2308	+ if ((status & handle_rx) && (chan < priv->plat->rx_queues_to_use)) {
	2309	+ if (napi_schedule_prep(&ch->rx_napi)) {
	2310	+ spin_lock_irqsave(&ch->lock, flags);
	2311	+ stmmac_disable_dma_irq(priv, priv->ioaddr, chan, 1, 0);
	2312	+ spin_unlock_irqrestore(&ch->lock, flags);
	2313	+ __napi_schedule(&ch->rx_napi);
	2314	+ }
2072	2315	}
2073	2316
2074		- if ((status & handle_tx) && ch->has_tx) {
2075		- needs_work = true;
2076		- } else {
2077		- status &= ~handle_tx;
2078		- }
2079		-
2080		- if (needs_work && napi_schedule_prep(&ch->napi)) {
2081		- stmmac_disable_dma_irq(priv, priv->ioaddr, chan);
2082		- __napi_schedule(&ch->napi);
	2317	+ if ((status & handle_tx) && (chan < priv->plat->tx_queues_to_use)) {
	2318	+ if (napi_schedule_prep(&ch->tx_napi)) {
	2319	+ spin_lock_irqsave(&ch->lock, flags);
	2320	+ stmmac_disable_dma_irq(priv, priv->ioaddr, chan, 0, 1);
	2321	+ spin_unlock_irqrestore(&ch->lock, flags);
	2322	+ __napi_schedule(&ch->tx_napi);
	2323	+ }
2083	2324	}
2084	2325
2085	2326	return status;
..	..	@@ -2142,10 +2383,10 @@
2142	2383	unsigned int mode = MMC_CNTRL_RESET_ON_READ \| MMC_CNTRL_COUNTER_RESET \|
2143	2384	MMC_CNTRL_PRESET \| MMC_CNTRL_FULL_HALF_PRESET;
2144	2385
2145		- dwmac_mmc_intr_all_mask(priv->mmcaddr);
	2386	+ stmmac_mmc_intr_all_mask(priv, priv->mmcaddr);
2146	2387
2147	2388	if (priv->dma_cap.rmon) {
2148		- dwmac_mmc_ctrl(priv->mmcaddr, mode);
	2389	+ stmmac_mmc_ctrl(priv, priv->mmcaddr, mode);
2149	2390	memset(&priv->mmc, 0, sizeof(struct stmmac_counters));
2150	2391	} else
2151	2392	netdev_info(priv->dev, "No MAC Management Counters available\n");
..	..	@@ -2174,7 +2415,7 @@
2174	2415	*/
2175	2416	static void stmmac_check_ether_addr(struct stmmac_priv *priv)
2176	2417	{
2177		- //if (!is_valid_ether_addr(priv->dev->dev_addr)) {
	2418	+// if (!is_valid_ether_addr(priv->dev->dev_addr)) {
2178	2419	if (1) {
2179	2420	stmmac_get_umac_addr(priv, priv->hw, priv->dev->dev_addr, 0);
2180	2421	if (likely(priv->plat->get_eth_addr))
..	..	@@ -2238,7 +2479,8 @@
2238	2479	rx_q->dma_rx_phy, chan);
2239	2480
2240	2481	rx_q->rx_tail_addr = rx_q->dma_rx_phy +
2241		- (DMA_RX_SIZE * sizeof(struct dma_desc));
	2482	+ (priv->dma_rx_size *
	2483	+ sizeof(struct dma_desc));
2242	2484	stmmac_set_rx_tail_ptr(priv, priv->ioaddr,
2243	2485	rx_q->rx_tail_addr, chan);
2244	2486	}
..	..	@@ -2267,7 +2509,7 @@
2267	2509
2268	2510	/**
2269	2511	* stmmac_tx_timer - mitigation sw timer for tx.
2270		- * @data: data pointer
	2512	+ * @t: data pointer
2271	2513	* Description:
2272	2514	* This is the timer handler to directly invoke the stmmac_tx_clean.
2273	2515	*/
..	..	@@ -2279,25 +2521,32 @@
2279	2521
2280	2522	ch = &priv->channel[tx_q->queue_index];
2281	2523
2282		- if (likely(napi_schedule_prep(&ch->napi)))
2283		- __napi_schedule(&ch->napi);
	2524	+ if (likely(napi_schedule_prep(&ch->tx_napi))) {
	2525	+ unsigned long flags;
	2526	+
	2527	+ spin_lock_irqsave(&ch->lock, flags);
	2528	+ stmmac_disable_dma_irq(priv, priv->ioaddr, ch->index, 0, 1);
	2529	+ spin_unlock_irqrestore(&ch->lock, flags);
	2530	+ __napi_schedule(&ch->tx_napi);
	2531	+ }
2284	2532	}
2285	2533
2286	2534	/**
2287		- * stmmac_init_tx_coalesce - init tx mitigation options.
	2535	+ * stmmac_init_coalesce - init mitigation options.
2288	2536	* @priv: driver private structure
2289	2537	* Description:
2290		- * This inits the transmit coalesce parameters: i.e. timer rate,
	2538	+ * This inits the coalesce parameters: i.e. timer rate,
2291	2539	* timer handler and default threshold used for enabling the
2292	2540	* interrupt on completion bit.
2293	2541	*/
2294		-static void stmmac_init_tx_coalesce(struct stmmac_priv *priv)
	2542	+static void stmmac_init_coalesce(struct stmmac_priv *priv)
2295	2543	{
2296	2544	u32 tx_channel_count = priv->plat->tx_queues_to_use;
2297	2545	u32 chan;
2298	2546
2299	2547	priv->tx_coal_frames = STMMAC_TX_FRAMES;
2300	2548	priv->tx_coal_timer = STMMAC_COAL_TX_TIMER;
	2549	+ priv->rx_coal_frames = STMMAC_RX_FRAMES;
2301	2550
2302	2551	for (chan = 0; chan < tx_channel_count; chan++) {
2303	2552	struct stmmac_tx_queue *tx_q = &priv->tx_queue[chan];
..	..	@@ -2315,12 +2564,12 @@
2315	2564	/* set TX ring length */
2316	2565	for (chan = 0; chan < tx_channels_count; chan++)
2317	2566	stmmac_set_tx_ring_len(priv, priv->ioaddr,
2318		- (DMA_TX_SIZE - 1), chan);
	2567	+ (priv->dma_tx_size - 1), chan);
2319	2568
2320	2569	/* set RX ring length */
2321	2570	for (chan = 0; chan < rx_channels_count; chan++)
2322	2571	stmmac_set_rx_ring_len(priv, priv->ioaddr,
2323		- (DMA_RX_SIZE - 1), chan);
	2572	+ (priv->dma_rx_size - 1), chan);
2324	2573	}
2325	2574
2326	2575	/**
..	..	@@ -2444,6 +2693,22 @@
2444	2693	}
2445	2694	}
2446	2695
	2696	+static void stmmac_mac_config_rss(struct stmmac_priv *priv)
	2697	+{
	2698	+ if (!priv->dma_cap.rssen \|\| !priv->plat->rss_en) {
	2699	+ priv->rss.enable = false;
	2700	+ return;
	2701	+ }
	2702	+
	2703	+ if (priv->dev->features & NETIF_F_RXHASH)
	2704	+ priv->rss.enable = true;
	2705	+ else
	2706	+ priv->rss.enable = false;
	2707	+
	2708	+ stmmac_rss_configure(priv, priv->hw, &priv->rss,
	2709	+ priv->plat->rx_queues_to_use);
	2710	+}
	2711	+
2447	2712	/**
2448	2713	* stmmac_mtl_configuration - Configure MTL
2449	2714	* @priv: driver private structure
..	..	@@ -2488,6 +2753,10 @@
2488	2753	/* Set RX routing */
2489	2754	if (rx_queues_count > 1)
2490	2755	stmmac_mac_config_rx_queues_routing(priv);
	2756	+
	2757	+ /* Receive Side Scaling */
	2758	+ if (rx_queues_count > 1)
	2759	+ stmmac_mac_config_rss(priv);
2491	2760	}
2492	2761
2493	2762	static void stmmac_safety_feat_configuration(struct stmmac_priv *priv)
..	..	@@ -2503,6 +2772,7 @@
2503	2772	/**
2504	2773	* stmmac_hw_setup - setup mac in a usable state.
2505	2774	* @dev : pointer to the device structure.
	2775	+ * @ptp_register: register PTP if set
2506	2776	* Description:
2507	2777	* this is the main function to setup the HW in a usable state because the
2508	2778	* dma engine is reset, the core registers are configured (e.g. AXI,
..	..	@@ -2512,7 +2782,7 @@
2512	2782	* 0 on success and an appropriate (-)ve integer as defined in errno.h
2513	2783	* file on failure.
2514	2784	*/
2515		-static int stmmac_hw_setup(struct net_device *dev, bool init_ptp)
	2785	+static int stmmac_hw_setup(struct net_device *dev, bool ptp_register)
2516	2786	{
2517	2787	struct stmmac_priv *priv = netdev_priv(dev);
2518	2788	u32 rx_cnt = priv->plat->rx_queues_to_use;
..	..	@@ -2568,37 +2838,75 @@
2568	2838
2569	2839	stmmac_mmc_setup(priv);
2570	2840
2571		- if (init_ptp) {
	2841	+ if (ptp_register) {
2572	2842	ret = clk_prepare_enable(priv->plat->clk_ptp_ref);
2573	2843	if (ret < 0)
2574		- netdev_warn(priv->dev, "failed to enable PTP reference clock: %d\n", ret);
2575		-
2576		- ret = stmmac_init_ptp(priv);
2577		- if (ret == -EOPNOTSUPP)
2578		- netdev_warn(priv->dev, "PTP not supported by HW\n");
2579		- else if (ret)
2580		- netdev_warn(priv->dev, "PTP init failed\n");
	2844	+ netdev_warn(priv->dev,
	2845	+ "failed to enable PTP reference clock: %pe\n",
	2846	+ ERR_PTR(ret));
2581	2847	}
2582	2848
2583		- priv->tx_lpi_timer = STMMAC_DEFAULT_TWT_LS;
	2849	+ ret = stmmac_init_ptp(priv);
	2850	+ if (ret == -EOPNOTSUPP)
	2851	+ netdev_warn(priv->dev, "PTP not supported by HW\n");
	2852	+ else if (ret)
	2853	+ netdev_warn(priv->dev, "PTP init failed\n");
	2854	+ else if (ptp_register)
	2855	+ stmmac_ptp_register(priv);
	2856	+
	2857	+ priv->eee_tw_timer = STMMAC_DEFAULT_TWT_LS;
	2858	+
	2859	+ /* Convert the timer from msec to usec */
	2860	+ if (!priv->tx_lpi_timer)
	2861	+ priv->tx_lpi_timer = eee_timer * 1000;
2584	2862
2585	2863	if (priv->use_riwt) {
2586		- ret = stmmac_rx_watchdog(priv, priv->ioaddr, MAX_DMA_RIWT, rx_cnt);
2587		- if (!ret)
2588		- priv->rx_riwt = MAX_DMA_RIWT;
	2864	+ if (!priv->rx_riwt)
	2865	+ priv->rx_riwt = DEF_DMA_RIWT;
	2866	+
	2867	+ ret = stmmac_rx_watchdog(priv, priv->ioaddr, priv->rx_riwt, rx_cnt);
2589	2868	}
2590	2869
2591	2870	if (priv->hw->pcs)
2592		- stmmac_pcs_ctrl_ane(priv, priv->hw, 1, priv->hw->ps, 0);
	2871	+ stmmac_pcs_ctrl_ane(priv, priv->ioaddr, 1, priv->hw->ps, 0);
2593	2872
2594	2873	/* set TX and RX rings length */
2595	2874	stmmac_set_rings_length(priv);
2596	2875
2597	2876	/* Enable TSO */
2598	2877	if (priv->tso) {
2599		- for (chan = 0; chan < tx_cnt; chan++)
	2878	+ for (chan = 0; chan < tx_cnt; chan++) {
	2879	+ struct stmmac_tx_queue *tx_q = &priv->tx_queue[chan];
	2880	+
	2881	+ /* TSO and TBS cannot co-exist */
	2882	+ if (tx_q->tbs & STMMAC_TBS_AVAIL)
	2883	+ continue;
	2884	+
2600	2885	stmmac_enable_tso(priv, priv->ioaddr, 1, chan);
	2886	+ }
2601	2887	}
	2888	+
	2889	+ /* Enable Split Header */
	2890	+ if (priv->sph && priv->hw->rx_csum) {
	2891	+ for (chan = 0; chan < rx_cnt; chan++)
	2892	+ stmmac_enable_sph(priv, priv->ioaddr, 1, chan);
	2893	+ }
	2894	+
	2895	+ /* VLAN Tag Insertion */
	2896	+ if (priv->dma_cap.vlins)
	2897	+ stmmac_enable_vlan(priv, priv->hw, STMMAC_VLAN_INSERT);
	2898	+
	2899	+ /* TBS */
	2900	+ for (chan = 0; chan < tx_cnt; chan++) {
	2901	+ struct stmmac_tx_queue *tx_q = &priv->tx_queue[chan];
	2902	+ int enable = tx_q->tbs & STMMAC_TBS_AVAIL;
	2903	+
	2904	+ stmmac_enable_tbs(priv, priv->ioaddr, enable, chan);
	2905	+ }
	2906	+
	2907	+ /* Configure real RX and TX queues */
	2908	+ netif_set_real_num_rx_queues(dev, priv->plat->rx_queues_to_use);
	2909	+ netif_set_real_num_tx_queues(dev, priv->plat->tx_queues_to_use);
2602	2910
2603	2911	/* Start the ball rolling... */
2604	2912	stmmac_start_all_dma(priv);
..	..	@@ -2625,18 +2933,26 @@
2625	2933	static int stmmac_open(struct net_device *dev)
2626	2934	{
2627	2935	struct stmmac_priv *priv = netdev_priv(dev);
	2936	+ int bfsize = 0;
2628	2937	u32 chan;
2629	2938	int ret;
2630	2939
2631		- if (priv->hw->pcs != STMMAC_PCS_RGMII &&
2632		- priv->hw->pcs != STMMAC_PCS_TBI &&
2633		- priv->hw->pcs != STMMAC_PCS_RTBI) {
	2940	+ //printk("ben:stmmac_open.. \n");
	2941	+ ret = pm_runtime_get_sync(priv->device);
	2942	+ if (ret < 0) {
	2943	+ pm_runtime_put_noidle(priv->device);
	2944	+ return ret;
	2945	+ }
	2946	+
	2947	+ if (priv->hw->pcs != STMMAC_PCS_TBI &&
	2948	+ priv->hw->pcs != STMMAC_PCS_RTBI &&
	2949	+ priv->hw->xpcs == NULL) {
2634	2950	ret = stmmac_init_phy(dev);
2635	2951	if (ret) {
2636	2952	netdev_err(priv->dev,
2637	2953	"%s: Cannot attach to PHY (error: %d)\n",
2638	2954	__func__, ret);
2639		- return ret;
	2955	+ goto init_phy_error;
2640	2956	}
2641	2957	}
2642	2958
..	..	@@ -2644,8 +2960,34 @@
2644	2960	memset(&priv->xstats, 0, sizeof(struct stmmac_extra_stats));
2645	2961	priv->xstats.threshold = tc;
2646	2962
2647		- priv->dma_buf_sz = STMMAC_ALIGN(buf_sz);
	2963	+ bfsize = stmmac_set_16kib_bfsize(priv, dev->mtu);
	2964	+ if (bfsize < 0)
	2965	+ bfsize = 0;
	2966	+
	2967	+ if (bfsize < BUF_SIZE_16KiB)
	2968	+ bfsize = stmmac_set_bfsize(dev->mtu, priv->dma_buf_sz);
	2969	+
	2970	+ priv->dma_buf_sz = bfsize;
	2971	+ buf_sz = bfsize;
	2972	+
2648	2973	priv->rx_copybreak = STMMAC_RX_COPYBREAK;
	2974	+
	2975	+ if (!priv->dma_tx_size)
	2976	+ priv->dma_tx_size = priv->plat->dma_tx_size ? priv->plat->dma_tx_size :
	2977	+ DMA_DEFAULT_TX_SIZE;
	2978	+
	2979	+ if (!priv->dma_rx_size)
	2980	+ priv->dma_rx_size = priv->plat->dma_rx_size ? priv->plat->dma_rx_size :
	2981	+ DMA_DEFAULT_RX_SIZE;
	2982	+
	2983	+ /* Earlier check for TBS */
	2984	+ for (chan = 0; chan < priv->plat->tx_queues_to_use; chan++) {
	2985	+ struct stmmac_tx_queue *tx_q = &priv->tx_queue[chan];
	2986	+ int tbs_en = priv->plat->tx_queues_cfg[chan].tbs_en;
	2987	+
	2988	+ /* Setup per-TXQ tbs flag before TX descriptor alloc */
	2989	+ tx_q->tbs \|= tbs_en ? STMMAC_TBS_AVAIL : 0;
	2990	+ }
2649	2991
2650	2992	ret = alloc_dma_desc_resources(priv);
2651	2993	if (ret < 0) {
..	..	@@ -2661,16 +3003,32 @@
2661	3003	goto init_error;
2662	3004	}
2663	3005
	3006	+ if (priv->plat->serdes_powerup) {
	3007	+ ret = priv->plat->serdes_powerup(dev, priv->plat->bsp_priv);
	3008	+ if (ret < 0) {
	3009	+ netdev_err(priv->dev, "%s: Serdes powerup failed\n",
	3010	+ __func__);
	3011	+ goto init_error;
	3012	+ }
	3013	+ }
	3014	+
	3015	+
	3016	+ #if 1
	3017	+ printk("ben -------bootup add 2s delay time.\n");
	3018	+ mdelay(2500);
	3019	+ #endif
	3020	+
2664	3021	ret = stmmac_hw_setup(dev, true);
2665	3022	if (ret < 0) {
2666	3023	netdev_err(priv->dev, "%s: Hw setup failed\n", __func__);
2667	3024	goto init_error;
2668	3025	}
2669	3026
2670		- stmmac_init_tx_coalesce(priv);
	3027	+ stmmac_init_coalesce(priv);
2671	3028
2672		- if (dev->phydev)
2673		- phy_start(dev->phydev);
	3029	+ phylink_start(priv->phylink);
	3030	+ /* We may have called phylink_speed_down before */
	3031	+ phylink_speed_up(priv->phylink);
2674	3032
2675	3033	/* Request the IRQ lines */
2676	3034	ret = request_irq(dev->irq, stmmac_interrupt,
..	..	@@ -2717,8 +3075,7 @@
2717	3075	wolirq_error:
2718	3076	free_irq(dev->irq, dev);
2719	3077	irq_error:
2720		- if (dev->phydev)
2721		- phy_stop(dev->phydev);
	3078	+ phylink_stop(priv->phylink);
2722	3079
2723	3080	for (chan = 0; chan < priv->plat->tx_queues_to_use; chan++)
2724	3081	del_timer_sync(&priv->tx_queue[chan].txtimer);
..	..	@@ -2727,9 +3084,9 @@
2727	3084	init_error:
2728	3085	free_dma_desc_resources(priv);
2729	3086	dma_desc_error:
2730		- if (dev->phydev)
2731		- phy_disconnect(dev->phydev);
2732		-
	3087	+ phylink_disconnect_phy(priv->phylink);
	3088	+init_phy_error:
	3089	+ pm_runtime_put(priv->device);
2733	3090	return ret;
2734	3091	}
2735	3092
..	..	@@ -2744,11 +3101,14 @@
2744	3101	struct stmmac_priv *priv = netdev_priv(dev);
2745	3102	u32 chan;
2746	3103
	3104	+ if (device_may_wakeup(priv->device))
	3105	+ phylink_speed_down(priv->phylink, false);
2747	3106	/* Stop and disconnect the PHY */
2748		- if (dev->phydev) {
2749		- phy_stop(dev->phydev);
2750		- phy_disconnect(dev->phydev);
2751		- }
	3107	+ phylink_stop(priv->phylink);
	3108	+ phylink_disconnect_phy(priv->phylink);
	3109	+
	3110	+ if (priv->plat->integrated_phy_power)
	3111	+ priv->plat->integrated_phy_power(priv->plat->bsp_priv, false);
2752	3112
2753	3113	stmmac_disable_all_queues(priv);
2754	3114
..	..	@@ -2776,11 +3136,48 @@
2776	3136	/* Disable the MAC Rx/Tx */
2777	3137	stmmac_mac_set(priv, priv->ioaddr, false);
2778	3138
	3139	+ /* Powerdown Serdes if there is */
	3140	+ if (priv->plat->serdes_powerdown)
	3141	+ priv->plat->serdes_powerdown(dev, priv->plat->bsp_priv);
	3142	+
2779	3143	netif_carrier_off(dev);
2780	3144
2781	3145	stmmac_release_ptp(priv);
2782	3146
	3147	+ pm_runtime_put(priv->device);
	3148	+
2783	3149	return 0;
	3150	+}
	3151	+
	3152	+static bool stmmac_vlan_insert(struct stmmac_priv priv, struct sk_buff skb,
	3153	+ struct stmmac_tx_queue *tx_q)
	3154	+{
	3155	+ u16 tag = 0x0, inner_tag = 0x0;
	3156	+ u32 inner_type = 0x0;
	3157	+ struct dma_desc *p;
	3158	+
	3159	+ if (!priv->dma_cap.vlins)
	3160	+ return false;
	3161	+ if (!skb_vlan_tag_present(skb))
	3162	+ return false;
	3163	+ if (skb->vlan_proto == htons(ETH_P_8021AD)) {
	3164	+ inner_tag = skb_vlan_tag_get(skb);
	3165	+ inner_type = STMMAC_VLAN_INSERT;
	3166	+ }
	3167	+
	3168	+ tag = skb_vlan_tag_get(skb);
	3169	+
	3170	+ if (tx_q->tbs & STMMAC_TBS_AVAIL)
	3171	+ p = &tx_q->dma_entx[tx_q->cur_tx].basic;
	3172	+ else
	3173	+ p = &tx_q->dma_tx[tx_q->cur_tx];
	3174	+
	3175	+ if (stmmac_set_desc_vlan_tag(priv, p, tag, inner_tag, inner_type))
	3176	+ return false;
	3177	+
	3178	+ stmmac_set_tx_owner(priv, p);
	3179	+ tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, priv->dma_tx_size);
	3180	+ return true;
2784	3181	}
2785	3182
2786	3183	/**
..	..	@@ -2788,13 +3185,13 @@
2788	3185	* @priv: driver private structure
2789	3186	* @des: buffer start address
2790	3187	* @total_len: total length to fill in descriptors
2791		- * @last_segmant: condition for the last descriptor
	3188	+ * @last_segment: condition for the last descriptor
2792	3189	* @queue: TX queue index
2793	3190	* Description:
2794	3191	* This function fills descriptor and request new descriptors according to
2795	3192	* buffer length to fill
2796	3193	*/
2797		-static void stmmac_tso_allocator(struct stmmac_priv *priv, unsigned int des,
	3194	+static void stmmac_tso_allocator(struct stmmac_priv *priv, dma_addr_t des,
2798	3195	int total_len, bool last_segment, u32 queue)
2799	3196	{
2800	3197	struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
..	..	@@ -2805,11 +3202,23 @@
2805	3202	tmp_len = total_len;
2806	3203
2807	3204	while (tmp_len > 0) {
2808		- tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, DMA_TX_SIZE);
2809		- WARN_ON(tx_q->tx_skbuff[tx_q->cur_tx]);
2810		- desc = tx_q->dma_tx + tx_q->cur_tx;
	3205	+ dma_addr_t curr_addr;
2811	3206
2812		- desc->des0 = cpu_to_le32(des + (total_len - tmp_len));
	3207	+ tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx,
	3208	+ priv->dma_tx_size);
	3209	+ WARN_ON(tx_q->tx_skbuff[tx_q->cur_tx]);
	3210	+
	3211	+ if (tx_q->tbs & STMMAC_TBS_AVAIL)
	3212	+ desc = &tx_q->dma_entx[tx_q->cur_tx].basic;
	3213	+ else
	3214	+ desc = &tx_q->dma_tx[tx_q->cur_tx];
	3215	+
	3216	+ curr_addr = des + (total_len - tmp_len);
	3217	+ if (priv->dma_cap.addr64 <= 32)
	3218	+ desc->des0 = cpu_to_le32(curr_addr);
	3219	+ else
	3220	+ stmmac_set_desc_addr(priv, desc, curr_addr);
	3221	+
2813	3222	buff_size = tmp_len >= TSO_MAX_BUFF_SIZE ?
2814	3223	TSO_MAX_BUFF_SIZE : tmp_len;
2815	3224
..	..	@@ -2853,16 +3262,19 @@
2853	3262	{
2854	3263	struct dma_desc desc, first, *mss_desc = NULL;
2855	3264	struct stmmac_priv *priv = netdev_priv(dev);
	3265	+ int desc_size, tmp_pay_len = 0, first_tx;
2856	3266	int nfrags = skb_shinfo(skb)->nr_frags;
2857	3267	u32 queue = skb_get_queue_mapping(skb);
2858		- unsigned int first_entry, des;
2859		- u8 proto_hdr_len, hdr;
	3268	+ unsigned int first_entry, tx_packets;
2860	3269	struct stmmac_tx_queue *tx_q;
2861		- int tmp_pay_len = 0;
	3270	+ bool has_vlan, set_ic;
	3271	+ u8 proto_hdr_len, hdr;
2862	3272	u32 pay_len, mss;
	3273	+ dma_addr_t des;
2863	3274	int i;
2864	3275
2865	3276	tx_q = &priv->tx_queue[queue];
	3277	+ first_tx = tx_q->cur_tx;
2866	3278
2867	3279	/* Compute header lengths */
2868	3280	if (skb_shinfo(skb)->gso_type & SKB_GSO_UDP_L4) {
..	..	@@ -2893,10 +3305,15 @@
2893	3305
2894	3306	/* set new MSS value if needed */
2895	3307	if (mss != tx_q->mss) {
2896		- mss_desc = tx_q->dma_tx + tx_q->cur_tx;
	3308	+ if (tx_q->tbs & STMMAC_TBS_AVAIL)
	3309	+ mss_desc = &tx_q->dma_entx[tx_q->cur_tx].basic;
	3310	+ else
	3311	+ mss_desc = &tx_q->dma_tx[tx_q->cur_tx];
	3312	+
2897	3313	stmmac_set_mss(priv, mss_desc, mss);
2898	3314	tx_q->mss = mss;
2899		- tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, DMA_TX_SIZE);
	3315	+ tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx,
	3316	+ priv->dma_tx_size);
2900	3317	WARN_ON(tx_q->tx_skbuff[tx_q->cur_tx]);
2901	3318	}
2902	3319
..	..	@@ -2907,11 +3324,20 @@
2907	3324	skb->data_len);
2908	3325	}
2909	3326
	3327	+ /* Check if VLAN can be inserted by HW */
	3328	+ has_vlan = stmmac_vlan_insert(priv, skb, tx_q);
	3329	+
2910	3330	first_entry = tx_q->cur_tx;
2911	3331	WARN_ON(tx_q->tx_skbuff[first_entry]);
2912	3332
2913		- desc = tx_q->dma_tx + first_entry;
	3333	+ if (tx_q->tbs & STMMAC_TBS_AVAIL)
	3334	+ desc = &tx_q->dma_entx[first_entry].basic;
	3335	+ else
	3336	+ desc = &tx_q->dma_tx[first_entry];
2914	3337	first = desc;
	3338	+
	3339	+ if (has_vlan)
	3340	+ stmmac_set_desc_vlan(priv, first, STMMAC_VLAN_INSERT);
2915	3341
2916	3342	/* first descriptor: fill Headers on Buf1 */
2917	3343	des = dma_map_single(priv->device, skb->data, skb_headlen(skb),
..	..	@@ -2922,14 +3348,21 @@
2922	3348	tx_q->tx_skbuff_dma[first_entry].buf = des;
2923	3349	tx_q->tx_skbuff_dma[first_entry].len = skb_headlen(skb);
2924	3350
2925		- first->des0 = cpu_to_le32(des);
	3351	+ if (priv->dma_cap.addr64 <= 32) {
	3352	+ first->des0 = cpu_to_le32(des);
2926	3353
2927		- /* Fill start of payload in buff2 of first descriptor */
2928		- if (pay_len)
2929		- first->des1 = cpu_to_le32(des + proto_hdr_len);
	3354	+ /* Fill start of payload in buff2 of first descriptor */
	3355	+ if (pay_len)
	3356	+ first->des1 = cpu_to_le32(des + proto_hdr_len);
2930	3357
2931		- /* If needed take extra descriptors to fill the remaining payload */
2932		- tmp_pay_len = pay_len - TSO_MAX_BUFF_SIZE;
	3358	+ /* If needed take extra descriptors to fill the remaining payload */
	3359	+ tmp_pay_len = pay_len - TSO_MAX_BUFF_SIZE;
	3360	+ } else {
	3361	+ stmmac_set_desc_addr(priv, first, des);
	3362	+ tmp_pay_len = pay_len;
	3363	+ des += proto_hdr_len;
	3364	+ pay_len = 0;
	3365	+ }
2933	3366
2934	3367	stmmac_tso_allocator(priv, des, tmp_pay_len, (nfrags == 0), queue);
2935	3368
..	..	@@ -2956,12 +3389,38 @@
2956	3389	/* Only the last descriptor gets to point to the skb. */
2957	3390	tx_q->tx_skbuff[tx_q->cur_tx] = skb;
2958	3391
	3392	+ /* Manage tx mitigation */
	3393	+ tx_packets = (tx_q->cur_tx + 1) - first_tx;
	3394	+ tx_q->tx_count_frames += tx_packets;
	3395	+
	3396	+ if ((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) && priv->hwts_tx_en)
	3397	+ set_ic = true;
	3398	+ else if (!priv->tx_coal_frames)
	3399	+ set_ic = false;
	3400	+ else if (tx_packets > priv->tx_coal_frames)
	3401	+ set_ic = true;
	3402	+ else if ((tx_q->tx_count_frames % priv->tx_coal_frames) < tx_packets)
	3403	+ set_ic = true;
	3404	+ else
	3405	+ set_ic = false;
	3406	+
	3407	+ if (set_ic) {
	3408	+ if (tx_q->tbs & STMMAC_TBS_AVAIL)
	3409	+ desc = &tx_q->dma_entx[tx_q->cur_tx].basic;
	3410	+ else
	3411	+ desc = &tx_q->dma_tx[tx_q->cur_tx];
	3412	+
	3413	+ tx_q->tx_count_frames = 0;
	3414	+ stmmac_set_tx_ic(priv, desc);
	3415	+ priv->xstats.tx_set_ic_bit++;
	3416	+ }
	3417	+
2959	3418	/* We've used all descriptors we need for this skb, however,
2960	3419	* advance cur_tx so that it references a fresh descriptor.
2961	3420	* ndo_start_xmit will fill this descriptor the next time it's
2962	3421	* called and stmmac_tx_clean may clean up to this descriptor.
2963	3422	*/
2964		- tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, DMA_TX_SIZE);
	3423	+ tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, priv->dma_tx_size);
2965	3424
2966	3425	if (unlikely(stmmac_tx_avail(priv, queue) <= (MAX_SKB_FRAGS + 1))) {
2967	3426	netif_dbg(priv, hw, priv->dev, "%s: stop transmitted packets\n",
..	..	@@ -2973,18 +3432,8 @@
2973	3432	priv->xstats.tx_tso_frames++;
2974	3433	priv->xstats.tx_tso_nfrags += nfrags;
2975	3434
2976		- /* Manage tx mitigation */
2977		- tx_q->tx_count_frames += nfrags + 1;
2978		- if (likely(priv->tx_coal_frames > tx_q->tx_count_frames) &&
2979		- !(priv->synopsys_id >= DWMAC_CORE_4_00 &&
2980		- (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
2981		- priv->hwts_tx_en)) {
2982		- stmmac_tx_timer_arm(priv, queue);
2983		- } else {
2984		- tx_q->tx_count_frames = 0;
2985		- stmmac_set_tx_ic(priv, desc);
2986		- priv->xstats.tx_set_ic_bit++;
2987		- }
	3435	+ if (priv->sarc_type)
	3436	+ stmmac_set_desc_sarc(priv, first, priv->sarc_type);
2988	3437
2989	3438	skb_tx_timestamp(skb);
2990	3439
..	..	@@ -3023,16 +3472,18 @@
3023	3472	pr_info("%s: curr=%d dirty=%d f=%d, e=%d, f_p=%p, nfrags %d\n",
3024	3473	__func__, tx_q->cur_tx, tx_q->dirty_tx, first_entry,
3025	3474	tx_q->cur_tx, first, nfrags);
3026		-
3027		- stmmac_display_ring(priv, (void *)tx_q->dma_tx, DMA_TX_SIZE, 0);
3028		-
3029	3475	pr_info(">>> frame to be transmitted: ");
3030	3476	print_pkt(skb->data, skb_headlen(skb));
3031	3477	}
3032	3478
3033	3479	netdev_tx_sent_queue(netdev_get_tx_queue(dev, queue), skb->len);
3034	3480
3035		- tx_q->tx_tail_addr = tx_q->dma_tx_phy + (tx_q->cur_tx * sizeof(*desc));
	3481	+ if (tx_q->tbs & STMMAC_TBS_AVAIL)
	3482	+ desc_size = sizeof(struct dma_edesc);
	3483	+ else
	3484	+ desc_size = sizeof(struct dma_desc);
	3485	+
	3486	+ tx_q->tx_tail_addr = tx_q->dma_tx_phy + (tx_q->cur_tx * desc_size);
3036	3487	stmmac_set_tx_tail_ptr(priv, priv->ioaddr, tx_q->tx_tail_addr, queue);
3037	3488	stmmac_tx_timer_arm(priv, queue);
3038	3489
..	..	@@ -3055,20 +3506,22 @@
3055	3506	*/
3056	3507	static netdev_tx_t stmmac_xmit(struct sk_buff skb, struct net_device dev)
3057	3508	{
	3509	+ unsigned int first_entry, tx_packets, enh_desc;
3058	3510	struct stmmac_priv *priv = netdev_priv(dev);
3059	3511	unsigned int nopaged_len = skb_headlen(skb);
3060	3512	int i, csum_insertion = 0, is_jumbo = 0;
3061	3513	u32 queue = skb_get_queue_mapping(skb);
3062	3514	int nfrags = skb_shinfo(skb)->nr_frags;
3063	3515	int gso = skb_shinfo(skb)->gso_type;
3064		- int entry;
3065		- unsigned int first_entry;
	3516	+ struct dma_edesc *tbs_desc = NULL;
	3517	+ int entry, desc_size, first_tx;
3066	3518	struct dma_desc desc, first;
3067	3519	struct stmmac_tx_queue *tx_q;
3068		- unsigned int enh_desc;
3069		- unsigned int des;
	3520	+ bool has_vlan, set_ic;
	3521	+ dma_addr_t des;
3070	3522
3071	3523	tx_q = &priv->tx_queue[queue];
	3524	+ first_tx = tx_q->cur_tx;
3072	3525
3073	3526	if (priv->tx_path_in_lpi_mode)
3074	3527	stmmac_disable_eee_mode(priv);
..	..	@@ -3093,6 +3546,9 @@
3093	3546	return NETDEV_TX_BUSY;
3094	3547	}
3095	3548
	3549	+ /* Check if VLAN can be inserted by HW */
	3550	+ has_vlan = stmmac_vlan_insert(priv, skb, tx_q);
	3551	+
3096	3552	entry = tx_q->cur_tx;
3097	3553	first_entry = entry;
3098	3554	WARN_ON(tx_q->tx_skbuff[first_entry]);
..	..	@@ -3101,10 +3557,15 @@
3101	3557
3102	3558	if (likely(priv->extend_desc))
3103	3559	desc = (struct dma_desc *)(tx_q->dma_etx + entry);
	3560	+ else if (tx_q->tbs & STMMAC_TBS_AVAIL)
	3561	+ desc = &tx_q->dma_entx[entry].basic;
3104	3562	else
3105	3563	desc = tx_q->dma_tx + entry;
3106	3564
3107	3565	first = desc;
	3566	+
	3567	+ if (has_vlan)
	3568	+ stmmac_set_desc_vlan(priv, first, STMMAC_VLAN_INSERT);
3108	3569
3109	3570	enh_desc = priv->plat->enh_desc;
3110	3571	/* To program the descriptors according to the size of the frame */
..	..	@@ -3122,11 +3583,13 @@
3122	3583	int len = skb_frag_size(frag);
3123	3584	bool last_segment = (i == (nfrags - 1));
3124	3585
3125		- entry = STMMAC_GET_ENTRY(entry, DMA_TX_SIZE);
	3586	+ entry = STMMAC_GET_ENTRY(entry, priv->dma_tx_size);
3126	3587	WARN_ON(tx_q->tx_skbuff[entry]);
3127	3588
3128	3589	if (likely(priv->extend_desc))
3129	3590	desc = (struct dma_desc *)(tx_q->dma_etx + entry);
	3591	+ else if (tx_q->tbs & STMMAC_TBS_AVAIL)
	3592	+ desc = &tx_q->dma_entx[entry].basic;
3130	3593	else
3131	3594	desc = tx_q->dma_tx + entry;
3132	3595
..	..	@@ -3151,28 +3614,51 @@
3151	3614	/* Only the last descriptor gets to point to the skb. */
3152	3615	tx_q->tx_skbuff[entry] = skb;
3153	3616
	3617	+ /* According to the coalesce parameter the IC bit for the latest
	3618	+ * segment is reset and the timer re-started to clean the tx status.
	3619	+ * This approach takes care about the fragments: desc is the first
	3620	+ * element in case of no SG.
	3621	+ */
	3622	+ tx_packets = (entry + 1) - first_tx;
	3623	+ tx_q->tx_count_frames += tx_packets;
	3624	+
	3625	+ if ((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) && priv->hwts_tx_en)
	3626	+ set_ic = true;
	3627	+ else if (!priv->tx_coal_frames)
	3628	+ set_ic = false;
	3629	+ else if (tx_packets > priv->tx_coal_frames)
	3630	+ set_ic = true;
	3631	+ else if ((tx_q->tx_count_frames % priv->tx_coal_frames) < tx_packets)
	3632	+ set_ic = true;
	3633	+ else
	3634	+ set_ic = false;
	3635	+
	3636	+ if (set_ic) {
	3637	+ if (likely(priv->extend_desc))
	3638	+ desc = &tx_q->dma_etx[entry].basic;
	3639	+ else if (tx_q->tbs & STMMAC_TBS_AVAIL)
	3640	+ desc = &tx_q->dma_entx[entry].basic;
	3641	+ else
	3642	+ desc = &tx_q->dma_tx[entry];
	3643	+
	3644	+ tx_q->tx_count_frames = 0;
	3645	+ stmmac_set_tx_ic(priv, desc);
	3646	+ priv->xstats.tx_set_ic_bit++;
	3647	+ }
	3648	+
3154	3649	/* We've used all descriptors we need for this skb, however,
3155	3650	* advance cur_tx so that it references a fresh descriptor.
3156	3651	* ndo_start_xmit will fill this descriptor the next time it's
3157	3652	* called and stmmac_tx_clean may clean up to this descriptor.
3158	3653	*/
3159		- entry = STMMAC_GET_ENTRY(entry, DMA_TX_SIZE);
	3654	+ entry = STMMAC_GET_ENTRY(entry, priv->dma_tx_size);
3160	3655	tx_q->cur_tx = entry;
3161	3656
3162	3657	if (netif_msg_pktdata(priv)) {
3163		- void *tx_head;
3164		-
3165	3658	netdev_dbg(priv->dev,
3166	3659	"%s: curr=%d dirty=%d f=%d, e=%d, first=%p, nfrags=%d",
3167	3660	__func__, tx_q->cur_tx, tx_q->dirty_tx, first_entry,
3168	3661	entry, first, nfrags);
3169		-
3170		- if (priv->extend_desc)
3171		- tx_head = (void *)tx_q->dma_etx;
3172		- else
3173		- tx_head = (void *)tx_q->dma_tx;
3174		-
3175		- stmmac_display_ring(priv, tx_head, DMA_TX_SIZE, false);
3176	3662
3177	3663	netdev_dbg(priv->dev, ">>> frame to be transmitted: ");
3178	3664	print_pkt(skb->data, skb->len);
..	..	@@ -3186,22 +3672,8 @@
3186	3672
3187	3673	dev->stats.tx_bytes += skb->len;
3188	3674
3189		- /* According to the coalesce parameter the IC bit for the latest
3190		- * segment is reset and the timer re-started to clean the tx status.
3191		- * This approach takes care about the fragments: desc is the first
3192		- * element in case of no SG.
3193		- */
3194		- tx_q->tx_count_frames += nfrags + 1;
3195		- if (likely(priv->tx_coal_frames > tx_q->tx_count_frames) &&
3196		- !(priv->synopsys_id >= DWMAC_CORE_4_00 &&
3197		- (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
3198		- priv->hwts_tx_en)) {
3199		- stmmac_tx_timer_arm(priv, queue);
3200		- } else {
3201		- tx_q->tx_count_frames = 0;
3202		- stmmac_set_tx_ic(priv, desc);
3203		- priv->xstats.tx_set_ic_bit++;
3204		- }
	3675	+ if (priv->sarc_type)
	3676	+ stmmac_set_desc_sarc(priv, first, priv->sarc_type);
3205	3677
3206	3678	skb_tx_timestamp(skb);
3207	3679
..	..	@@ -3233,11 +3705,18 @@
3233	3705
3234	3706	/* Prepare the first descriptor setting the OWN bit too */
3235	3707	stmmac_prepare_tx_desc(priv, first, 1, nopaged_len,
3236		- csum_insertion, priv->mode, 1, last_segment,
	3708	+ csum_insertion, priv->mode, 0, last_segment,
3237	3709	skb->len);
3238		- } else {
3239		- stmmac_set_tx_owner(priv, first);
3240	3710	}
	3711	+
	3712	+ if (tx_q->tbs & STMMAC_TBS_EN) {
	3713	+ struct timespec64 ts = ns_to_timespec64(skb->tstamp);
	3714	+
	3715	+ tbs_desc = &tx_q->dma_entx[first_entry];
	3716	+ stmmac_set_desc_tbs(priv, tbs_desc, ts.tv_sec, ts.tv_nsec);
	3717	+ }
	3718	+
	3719	+ stmmac_set_tx_owner(priv, first);
3241	3720
3242	3721	/* The own bit must be the latest setting done when prepare the
3243	3722	* descriptor and then barrier is needed to make sure that
..	..	@@ -3249,7 +3728,14 @@
3249	3728
3250	3729	stmmac_enable_dma_transmission(priv, priv->ioaddr);
3251	3730
3252		- tx_q->tx_tail_addr = tx_q->dma_tx_phy + (tx_q->cur_tx * sizeof(*desc));
	3731	+ if (likely(priv->extend_desc))
	3732	+ desc_size = sizeof(struct dma_extended_desc);
	3733	+ else if (tx_q->tbs & STMMAC_TBS_AVAIL)
	3734	+ desc_size = sizeof(struct dma_edesc);
	3735	+ else
	3736	+ desc_size = sizeof(struct dma_desc);
	3737	+
	3738	+ tx_q->tx_tail_addr = tx_q->dma_tx_phy + (tx_q->cur_tx * desc_size);
3253	3739	stmmac_set_tx_tail_ptr(priv, priv->ioaddr, tx_q->tx_tail_addr, queue);
3254	3740	stmmac_tx_timer_arm(priv, queue);
3255	3741
..	..	@@ -3283,15 +3769,6 @@
3283	3769	}
3284	3770	}
3285	3771
3286		-
3287		-static inline int stmmac_rx_threshold_count(struct stmmac_rx_queue *rx_q)
3288		-{
3289		- if (rx_q->rx_zeroc_thresh < STMMAC_RX_THRESH)
3290		- return 0;
3291		-
3292		- return 1;
3293		-}
3294		-
3295	3772	/**
3296	3773	* stmmac_rx_refill - refill used skb preallocated buffers
3297	3774	* @priv: driver private structure
..	..	@@ -3302,63 +3779,115 @@
3302	3779	static inline void stmmac_rx_refill(struct stmmac_priv *priv, u32 queue)
3303	3780	{
3304	3781	struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
3305		- int dirty = stmmac_rx_dirty(priv, queue);
	3782	+ int len, dirty = stmmac_rx_dirty(priv, queue);
3306	3783	unsigned int entry = rx_q->dirty_rx;
	3784	+ gfp_t gfp = (GFP_ATOMIC \| __GFP_NOWARN);
3307	3785
3308		- int bfsize = priv->dma_buf_sz;
	3786	+ if (priv->dma_cap.addr64 <= 32)
	3787	+ gfp \|= GFP_DMA32;
	3788	+
	3789	+ len = DIV_ROUND_UP(priv->dma_buf_sz, PAGE_SIZE) * PAGE_SIZE;
3309	3790
3310	3791	while (dirty-- > 0) {
	3792	+ struct stmmac_rx_buffer *buf = &rx_q->buf_pool[entry];
3311	3793	struct dma_desc *p;
	3794	+ bool use_rx_wd;
3312	3795
3313	3796	if (priv->extend_desc)
3314	3797	p = (struct dma_desc *)(rx_q->dma_erx + entry);
3315	3798	else
3316	3799	p = rx_q->dma_rx + entry;
3317	3800
3318		- if (likely(!rx_q->rx_skbuff[entry])) {
3319		- struct sk_buff *skb;
3320		-
3321		- skb = netdev_alloc_skb_ip_align(priv->dev, bfsize);
3322		- if (unlikely(!skb)) {
3323		- /* so for a while no zero-copy! */
3324		- rx_q->rx_zeroc_thresh = STMMAC_RX_THRESH;
3325		- if (unlikely(net_ratelimit()))
3326		- dev_err(priv->device,
3327		- "fail to alloc skb entry %d\n",
3328		- entry);
	3801	+ if (!buf->page) {
	3802	+ buf->page = page_pool_alloc_pages(rx_q->page_pool, gfp);
	3803	+ if (!buf->page)
3329	3804	break;
3330		- }
3331		-
3332		- rx_q->rx_skbuff[entry] = skb;
3333		- rx_q->rx_skbuff_dma[entry] =
3334		- dma_map_single(priv->device, skb->data, bfsize,
3335		- DMA_FROM_DEVICE);
3336		- if (dma_mapping_error(priv->device,
3337		- rx_q->rx_skbuff_dma[entry])) {
3338		- netdev_err(priv->dev, "Rx DMA map failed\n");
3339		- dev_kfree_skb(skb);
3340		- break;
3341		- }
3342		-
3343		- stmmac_set_desc_addr(priv, p, rx_q->rx_skbuff_dma[entry]);
3344		- stmmac_refill_desc3(priv, rx_q, p);
3345		-
3346		- if (rx_q->rx_zeroc_thresh > 0)
3347		- rx_q->rx_zeroc_thresh--;
3348		-
3349		- netif_dbg(priv, rx_status, priv->dev,
3350		- "refill entry #%d\n", entry);
3351	3805	}
3352		- dma_wmb();
3353	3806
3354		- stmmac_set_rx_owner(priv, p, priv->use_riwt);
	3807	+ if (priv->sph && !buf->sec_page) {
	3808	+ buf->sec_page = page_pool_alloc_pages(rx_q->page_pool, gfp);
	3809	+ if (!buf->sec_page)
	3810	+ break;
	3811	+
	3812	+ buf->sec_addr = page_pool_get_dma_addr(buf->sec_page);
	3813	+ }
	3814	+
	3815	+ buf->addr = page_pool_get_dma_addr(buf->page);
	3816	+ stmmac_set_desc_addr(priv, p, buf->addr);
	3817	+ if (priv->sph)
	3818	+ stmmac_set_desc_sec_addr(priv, p, buf->sec_addr, true);
	3819	+ else
	3820	+ stmmac_set_desc_sec_addr(priv, p, buf->sec_addr, false);
	3821	+ stmmac_refill_desc3(priv, rx_q, p);
	3822	+
	3823	+ rx_q->rx_count_frames++;
	3824	+ rx_q->rx_count_frames += priv->rx_coal_frames;
	3825	+ if (rx_q->rx_count_frames > priv->rx_coal_frames)
	3826	+ rx_q->rx_count_frames = 0;
	3827	+
	3828	+ use_rx_wd = !priv->rx_coal_frames;
	3829	+ use_rx_wd \|= rx_q->rx_count_frames > 0;
	3830	+ if (!priv->use_riwt)
	3831	+ use_rx_wd = false;
3355	3832
3356	3833	dma_wmb();
	3834	+ stmmac_set_rx_owner(priv, p, use_rx_wd);
3357	3835
3358		- entry = STMMAC_GET_ENTRY(entry, DMA_RX_SIZE);
	3836	+ entry = STMMAC_GET_ENTRY(entry, priv->dma_rx_size);
3359	3837	}
3360	3838	rx_q->dirty_rx = entry;
	3839	+ rx_q->rx_tail_addr = rx_q->dma_rx_phy +
	3840	+ (rx_q->dirty_rx * sizeof(struct dma_desc));
3361	3841	stmmac_set_rx_tail_ptr(priv, priv->ioaddr, rx_q->rx_tail_addr, queue);
	3842	+}
	3843	+
	3844	+static unsigned int stmmac_rx_buf1_len(struct stmmac_priv *priv,
	3845	+ struct dma_desc *p,
	3846	+ int status, unsigned int len)
	3847	+{
	3848	+ unsigned int plen = 0, hlen = 0;
	3849	+ int coe = priv->hw->rx_csum;
	3850	+
	3851	+ /* Not first descriptor, buffer is always zero */
	3852	+ if (priv->sph && len)
	3853	+ return 0;
	3854	+
	3855	+ /* First descriptor, get split header length */
	3856	+ stmmac_get_rx_header_len(priv, p, &hlen);
	3857	+ if (priv->sph && hlen) {
	3858	+ priv->xstats.rx_split_hdr_pkt_n++;
	3859	+ return hlen;
	3860	+ }
	3861	+
	3862	+ /* First descriptor, not last descriptor and not split header */
	3863	+ if (status & rx_not_ls)
	3864	+ return priv->dma_buf_sz;
	3865	+
	3866	+ plen = stmmac_get_rx_frame_len(priv, p, coe);
	3867	+
	3868	+ /* First descriptor and last descriptor and not split header */
	3869	+ return min_t(unsigned int, priv->dma_buf_sz, plen);
	3870	+}
	3871	+
	3872	+static unsigned int stmmac_rx_buf2_len(struct stmmac_priv *priv,
	3873	+ struct dma_desc *p,
	3874	+ int status, unsigned int len)
	3875	+{
	3876	+ int coe = priv->hw->rx_csum;
	3877	+ unsigned int plen = 0;
	3878	+
	3879	+ /* Not split header, buffer is not available */
	3880	+ if (!priv->sph)
	3881	+ return 0;
	3882	+
	3883	+ /* Not last descriptor */
	3884	+ if (status & rx_not_ls)
	3885	+ return priv->dma_buf_sz;
	3886	+
	3887	+ plen = stmmac_get_rx_frame_len(priv, p, coe);
	3888	+
	3889	+ /* Last descriptor */
	3890	+ return plen - len;
3362	3891	}
3363	3892
3364	3893	/**
..	..	@@ -3373,30 +3902,54 @@
3373	3902	{
3374	3903	struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
3375	3904	struct stmmac_channel *ch = &priv->channel[queue];
	3905	+ unsigned int count = 0, error = 0, len = 0;
	3906	+ int status = 0, coe = priv->hw->rx_csum;
3376	3907	unsigned int next_entry = rx_q->cur_rx;
3377		- int coe = priv->hw->rx_csum;
3378		- unsigned int count = 0;
3379		- bool xmac;
3380		-
3381		- xmac = priv->plat->has_gmac4 \|\| priv->plat->has_xgmac;
	3908	+ unsigned int desc_size;
	3909	+ struct sk_buff *skb = NULL;
3382	3910
3383	3911	if (netif_msg_rx_status(priv)) {
3384	3912	void *rx_head;
3385	3913
3386	3914	netdev_dbg(priv->dev, "%s: descriptor ring:\n", __func__);
3387		- if (priv->extend_desc)
	3915	+ if (priv->extend_desc) {
3388	3916	rx_head = (void *)rx_q->dma_erx;
3389		- else
	3917	+ desc_size = sizeof(struct dma_extended_desc);
	3918	+ } else {
3390	3919	rx_head = (void *)rx_q->dma_rx;
	3920	+ desc_size = sizeof(struct dma_desc);
	3921	+ }
3391	3922
3392		- stmmac_display_ring(priv, rx_head, DMA_RX_SIZE, true);
	3923	+ stmmac_display_ring(priv, rx_head, priv->dma_rx_size, true,
	3924	+ rx_q->dma_rx_phy, desc_size);
3393	3925	}
3394	3926	while (count < limit) {
3395		- int entry, status;
3396		- struct dma_desc *p;
3397		- struct dma_desc *np;
	3927	+ unsigned int buf1_len = 0, buf2_len = 0;
	3928	+ enum pkt_hash_types hash_type;
	3929	+ struct stmmac_rx_buffer *buf;
	3930	+ struct dma_desc np, p;
	3931	+ int entry;
	3932	+ u32 hash;
3398	3933
	3934	+ if (!count && rx_q->state_saved) {
	3935	+ skb = rx_q->state.skb;
	3936	+ error = rx_q->state.error;
	3937	+ len = rx_q->state.len;
	3938	+ } else {
	3939	+ rx_q->state_saved = false;
	3940	+ skb = NULL;
	3941	+ error = 0;
	3942	+ len = 0;
	3943	+ }
	3944	+
	3945	+ if ((count >= limit - 1) && limit > 1)
	3946	+ break;
	3947	+
	3948	+read_again:
	3949	+ buf1_len = 0;
	3950	+ buf2_len = 0;
3399	3951	entry = next_entry;
	3952	+ buf = &rx_q->buf_pool[entry];
3400	3953
3401	3954	if (priv->extend_desc)
3402	3955	p = (struct dma_desc *)(rx_q->dma_erx + entry);
..	..	@@ -3410,9 +3963,8 @@
3410	3963	if (unlikely(status & dma_own))
3411	3964	break;
3412	3965
3413		- count++;
3414		-
3415		- rx_q->cur_rx = STMMAC_GET_ENTRY(rx_q->cur_rx, DMA_RX_SIZE);
	3966	+ rx_q->cur_rx = STMMAC_GET_ENTRY(rx_q->cur_rx,
	3967	+ priv->dma_rx_size);
3416	3968	next_entry = rx_q->cur_rx;
3417	3969
3418	3970	if (priv->extend_desc)
..	..	@@ -3426,133 +3978,126 @@
3426	3978	stmmac_rx_extended_status(priv, &priv->dev->stats,
3427	3979	&priv->xstats, rx_q->dma_erx + entry);
3428	3980	if (unlikely(status == discard_frame)) {
3429		- priv->dev->stats.rx_errors++;
3430		- if (priv->hwts_rx_en && !priv->extend_desc) {
3431		- /* DESC2 & DESC3 will be overwritten by device
3432		- * with timestamp value, hence reinitialize
3433		- * them in stmmac_rx_refill() function so that
3434		- * device can reuse it.
3435		- */
3436		- dev_kfree_skb_any(rx_q->rx_skbuff[entry]);
3437		- rx_q->rx_skbuff[entry] = NULL;
3438		- dma_unmap_single(priv->device,
3439		- rx_q->rx_skbuff_dma[entry],
3440		- priv->dma_buf_sz,
3441		- DMA_FROM_DEVICE);
3442		- }
3443		- } else {
3444		- struct sk_buff *skb;
3445		- int frame_len;
3446		- unsigned int des;
3447		-
3448		- stmmac_get_desc_addr(priv, p, &des);
3449		- frame_len = stmmac_get_rx_frame_len(priv, p, coe);
3450		-
3451		- /* If frame length is greater than skb buffer size
3452		- * (preallocated during init) then the packet is
3453		- * ignored
3454		- */
3455		- if (frame_len > priv->dma_buf_sz) {
3456		- if (net_ratelimit())
3457		- netdev_err(priv->dev,
3458		- "len %d larger than size (%d)\n",
3459		- frame_len, priv->dma_buf_sz);
3460		- priv->dev->stats.rx_length_errors++;
3461		- continue;
3462		- }
3463		-
3464		- /* ACS is set; GMAC core strips PAD/FCS for IEEE 802.3
3465		- * Type frames (LLC/LLC-SNAP)
3466		- *
3467		- * llc_snap is never checked in GMAC >= 4, so this ACS
3468		- * feature is always disabled and packets need to be
3469		- * stripped manually.
3470		- */
3471		- if (unlikely(priv->synopsys_id >= DWMAC_CORE_4_00) \|\|
3472		- unlikely(status != llc_snap))
3473		- frame_len -= ETH_FCS_LEN;
3474		-
3475		- if (netif_msg_rx_status(priv)) {
3476		- netdev_dbg(priv->dev, "\tdesc: %p [entry %d] buff=0x%x\n",
3477		- p, entry, des);
3478		- netdev_dbg(priv->dev, "frame size %d, COE: %d\n",
3479		- frame_len, status);
3480		- }
3481		-
3482		- /* The zero-copy is always used for all the sizes
3483		- * in case of GMAC4 because it needs
3484		- * to refill the used descriptors, always.
3485		- */
3486		- if (unlikely(!xmac &&
3487		- ((frame_len < priv->rx_copybreak) \|\|
3488		- stmmac_rx_threshold_count(rx_q)))) {
3489		- skb = netdev_alloc_skb_ip_align(priv->dev,
3490		- frame_len);
3491		- if (unlikely(!skb)) {
3492		- if (net_ratelimit())
3493		- dev_warn(priv->device,
3494		- "packet dropped\n");
3495		- priv->dev->stats.rx_dropped++;
3496		- continue;
3497		- }
3498		-
3499		- dma_sync_single_for_cpu(priv->device,
3500		- rx_q->rx_skbuff_dma
3501		- [entry], frame_len,
3502		- DMA_FROM_DEVICE);
3503		- skb_copy_to_linear_data(skb,
3504		- rx_q->
3505		- rx_skbuff[entry]->data,
3506		- frame_len);
3507		-
3508		- skb_put(skb, frame_len);
3509		- dma_sync_single_for_device(priv->device,
3510		- rx_q->rx_skbuff_dma
3511		- [entry], frame_len,
3512		- DMA_FROM_DEVICE);
3513		- } else {
3514		- skb = rx_q->rx_skbuff[entry];
3515		- if (unlikely(!skb)) {
3516		- if (net_ratelimit())
3517		- netdev_err(priv->dev,
3518		- "%s: Inconsistent Rx chain\n",
3519		- priv->dev->name);
3520		- priv->dev->stats.rx_dropped++;
3521		- continue;
3522		- }
3523		- prefetch(skb->data - NET_IP_ALIGN);
3524		- rx_q->rx_skbuff[entry] = NULL;
3525		- rx_q->rx_zeroc_thresh++;
3526		-
3527		- skb_put(skb, frame_len);
3528		- dma_unmap_single(priv->device,
3529		- rx_q->rx_skbuff_dma[entry],
3530		- priv->dma_buf_sz,
3531		- DMA_FROM_DEVICE);
3532		- }
3533		-
3534		- if (netif_msg_pktdata(priv)) {
3535		- netdev_dbg(priv->dev, "frame received (%dbytes)",
3536		- frame_len);
3537		- print_pkt(skb->data, frame_len);
3538		- }
3539		-
3540		- stmmac_get_rx_hwtstamp(priv, p, np, skb);
3541		-
3542		- stmmac_rx_vlan(priv->dev, skb);
3543		-
3544		- skb->protocol = eth_type_trans(skb, priv->dev);
3545		-
3546		- if (unlikely(!coe))
3547		- skb_checksum_none_assert(skb);
3548		- else
3549		- skb->ip_summed = CHECKSUM_UNNECESSARY;
3550		-
3551		- napi_gro_receive(&ch->napi, skb);
3552		-
3553		- priv->dev->stats.rx_packets++;
3554		- priv->dev->stats.rx_bytes += frame_len;
	3981	+ page_pool_recycle_direct(rx_q->page_pool, buf->page);
	3982	+ buf->page = NULL;
	3983	+ error = 1;
	3984	+ if (!priv->hwts_rx_en)
	3985	+ priv->dev->stats.rx_errors++;
3555	3986	}
	3987	+
	3988	+ if (unlikely(error && (status & rx_not_ls)))
	3989	+ goto read_again;
	3990	+ if (unlikely(error)) {
	3991	+ dev_kfree_skb(skb);
	3992	+ skb = NULL;
	3993	+ count++;
	3994	+ continue;
	3995	+ }
	3996	+
	3997	+ /* Buffer is good. Go on. */
	3998	+
	3999	+ prefetch(page_address(buf->page));
	4000	+ if (buf->sec_page)
	4001	+ prefetch(page_address(buf->sec_page));
	4002	+
	4003	+ buf1_len = stmmac_rx_buf1_len(priv, p, status, len);
	4004	+ len += buf1_len;
	4005	+ buf2_len = stmmac_rx_buf2_len(priv, p, status, len);
	4006	+ len += buf2_len;
	4007	+
	4008	+ /* ACS is set; GMAC core strips PAD/FCS for IEEE 802.3
	4009	+ * Type frames (LLC/LLC-SNAP)
	4010	+ *
	4011	+ * llc_snap is never checked in GMAC >= 4, so this ACS
	4012	+ * feature is always disabled and packets need to be
	4013	+ * stripped manually.
	4014	+ */
	4015	+ if (likely(!(status & rx_not_ls)) &&
	4016	+ (likely(priv->synopsys_id >= DWMAC_CORE_4_00) \|\|
	4017	+ unlikely(status != llc_snap))) {
	4018	+ if (buf2_len)
	4019	+ buf2_len -= ETH_FCS_LEN;
	4020	+ else
	4021	+ buf1_len -= ETH_FCS_LEN;
	4022	+
	4023	+ len -= ETH_FCS_LEN;
	4024	+ }
	4025	+
	4026	+ if (!skb) {
	4027	+ skb = napi_alloc_skb(&ch->rx_napi, buf1_len);
	4028	+ if (!skb) {
	4029	+ priv->dev->stats.rx_dropped++;
	4030	+ count++;
	4031	+ goto drain_data;
	4032	+ }
	4033	+
	4034	+ dma_sync_single_for_cpu(priv->device, buf->addr,
	4035	+ buf1_len, DMA_FROM_DEVICE);
	4036	+ skb_copy_to_linear_data(skb, page_address(buf->page),
	4037	+ buf1_len);
	4038	+ skb_put(skb, buf1_len);
	4039	+
	4040	+ /* Data payload copied into SKB, page ready for recycle */
	4041	+ page_pool_recycle_direct(rx_q->page_pool, buf->page);
	4042	+ buf->page = NULL;
	4043	+ } else if (buf1_len) {
	4044	+ dma_sync_single_for_cpu(priv->device, buf->addr,
	4045	+ buf1_len, DMA_FROM_DEVICE);
	4046	+ skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
	4047	+ buf->page, 0, buf1_len,
	4048	+ priv->dma_buf_sz);
	4049	+
	4050	+ /* Data payload appended into SKB */
	4051	+ page_pool_release_page(rx_q->page_pool, buf->page);
	4052	+ buf->page = NULL;
	4053	+ }
	4054	+
	4055	+ if (buf2_len) {
	4056	+ dma_sync_single_for_cpu(priv->device, buf->sec_addr,
	4057	+ buf2_len, DMA_FROM_DEVICE);
	4058	+ skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
	4059	+ buf->sec_page, 0, buf2_len,
	4060	+ priv->dma_buf_sz);
	4061	+
	4062	+ /* Data payload appended into SKB */
	4063	+ page_pool_release_page(rx_q->page_pool, buf->sec_page);
	4064	+ buf->sec_page = NULL;
	4065	+ }
	4066	+
	4067	+drain_data:
	4068	+ if (likely(status & rx_not_ls))
	4069	+ goto read_again;
	4070	+ if (!skb)
	4071	+ continue;
	4072	+
	4073	+ /* Got entire packet into SKB. Finish it. */
	4074	+
	4075	+ stmmac_get_rx_hwtstamp(priv, p, np, skb);
	4076	+ stmmac_rx_vlan(priv->dev, skb);
	4077	+ skb->protocol = eth_type_trans(skb, priv->dev);
	4078	+
	4079	+ if (unlikely(!coe))
	4080	+ skb_checksum_none_assert(skb);
	4081	+ else
	4082	+ skb->ip_summed = CHECKSUM_UNNECESSARY;
	4083	+
	4084	+ if (!stmmac_get_rx_hash(priv, p, &hash, &hash_type))
	4085	+ skb_set_hash(skb, hash, hash_type);
	4086	+
	4087	+ skb_record_rx_queue(skb, queue);
	4088	+ napi_gro_receive(&ch->rx_napi, skb);
	4089	+ skb = NULL;
	4090	+
	4091	+ priv->dev->stats.rx_packets++;
	4092	+ priv->dev->stats.rx_bytes += len;
	4093	+ count++;
	4094	+ }
	4095	+
	4096	+ if (status & rx_not_ls \|\| skb) {
	4097	+ rx_q->state_saved = true;
	4098	+ rx_q->state.skb = skb;
	4099	+ rx_q->state.error = error;
	4100	+ rx_q->state.len = len;
3556	4101	}
3557	4102
3558	4103	stmmac_rx_refill(priv, queue);
..	..	@@ -3562,40 +4107,47 @@
3562	4107	return count;
3563	4108	}
3564	4109
3565		-/**
3566		- * stmmac_poll - stmmac poll method (NAPI)
3567		- * @napi : pointer to the napi structure.
3568		- * @budget : maximum number of packets that the current CPU can receive from
3569		- * all interfaces.
3570		- * Description :
3571		- * To look at the incoming frames and clear the tx resources.
3572		- */
3573		-static int stmmac_napi_poll(struct napi_struct *napi, int budget)
	4110	+static int stmmac_napi_poll_rx(struct napi_struct *napi, int budget)
3574	4111	{
3575	4112	struct stmmac_channel *ch =
3576		- container_of(napi, struct stmmac_channel, napi);
	4113	+ container_of(napi, struct stmmac_channel, rx_napi);
3577	4114	struct stmmac_priv *priv = ch->priv_data;
3578		- int work_done, rx_done = 0, tx_done = 0;
3579	4115	u32 chan = ch->index;
	4116	+ int work_done;
3580	4117
3581	4118	priv->xstats.napi_poll++;
3582	4119
3583		- if (ch->has_tx)
3584		- tx_done = stmmac_tx_clean(priv, budget, chan);
3585		- if (ch->has_rx)
3586		- rx_done = stmmac_rx(priv, budget, chan);
	4120	+ work_done = stmmac_rx(priv, budget, chan);
	4121	+ if (work_done < budget && napi_complete_done(napi, work_done)) {
	4122	+ unsigned long flags;
3587	4123
3588		- work_done = max(rx_done, tx_done);
	4124	+ spin_lock_irqsave(&ch->lock, flags);
	4125	+ stmmac_enable_dma_irq(priv, priv->ioaddr, chan, 1, 0);
	4126	+ spin_unlock_irqrestore(&ch->lock, flags);
	4127	+ }
	4128	+
	4129	+ return work_done;
	4130	+}
	4131	+
	4132	+static int stmmac_napi_poll_tx(struct napi_struct *napi, int budget)
	4133	+{
	4134	+ struct stmmac_channel *ch =
	4135	+ container_of(napi, struct stmmac_channel, tx_napi);
	4136	+ struct stmmac_priv *priv = ch->priv_data;
	4137	+ u32 chan = ch->index;
	4138	+ int work_done;
	4139	+
	4140	+ priv->xstats.napi_poll++;
	4141	+
	4142	+ work_done = stmmac_tx_clean(priv, priv->dma_tx_size, chan);
3589	4143	work_done = min(work_done, budget);
3590	4144
3591	4145	if (work_done < budget && napi_complete_done(napi, work_done)) {
3592		- int stat;
	4146	+ unsigned long flags;
3593	4147
3594		- stmmac_enable_dma_irq(priv, priv->ioaddr, chan);
3595		- stat = stmmac_dma_interrupt_status(priv, priv->ioaddr,
3596		- &priv->xstats, chan);
3597		- if (stat && napi_reschedule(napi))
3598		- stmmac_disable_dma_irq(priv, priv->ioaddr, chan);
	4148	+ spin_lock_irqsave(&ch->lock, flags);
	4149	+ stmmac_enable_dma_irq(priv, priv->ioaddr, chan, 0, 1);
	4150	+ spin_unlock_irqrestore(&ch->lock, flags);
3599	4151	}
3600	4152
3601	4153	return work_done;
..	..	@@ -3604,12 +4156,13 @@
3604	4156	/**
3605	4157	* stmmac_tx_timeout
3606	4158	* @dev : Pointer to net device structure
	4159	+ * @txqueue: the index of the hanging transmit queue
3607	4160	* Description: this function is called when a packet transmission fails to
3608	4161	* complete within a reasonable time. The driver will mark the error in the
3609	4162	* netdev structure and arrange for the device to be reset to a sane state
3610	4163	* in order to transmit a new packet.
3611	4164	*/
3612		-static void stmmac_tx_timeout(struct net_device *dev)
	4165	+static void stmmac_tx_timeout(struct net_device *dev, unsigned int txqueue)
3613	4166	{
3614	4167	struct stmmac_priv *priv = netdev_priv(dev);
3615	4168
..	..	@@ -3706,6 +4259,8 @@
3706	4259	netdev_features_t features)
3707	4260	{
3708	4261	struct stmmac_priv *priv = netdev_priv(netdev);
	4262	+ bool sph_en;
	4263	+ u32 chan;
3709	4264
3710	4265	/* Keep the COE Type in case of csum is supporting */
3711	4266	if (features & NETIF_F_RXCSUM)
..	..	@@ -3716,6 +4271,10 @@
3716	4271	* fixed in case of issue.
3717	4272	*/
3718	4273	stmmac_rx_ipc(priv, priv->hw);
	4274	+
	4275	+ sph_en = (priv->hw->rx_csum > 0) && priv->sph;
	4276	+ for (chan = 0; chan < priv->plat->rx_queues_to_use; chan++)
	4277	+ stmmac_enable_sph(priv, priv->ioaddr, sph_en, chan);
3719	4278
3720	4279	return 0;
3721	4280	}
..	..	@@ -3757,7 +4316,6 @@
3757	4316	/* To handle GMAC own interrupts */
3758	4317	if ((priv->plat->has_gmac) \|\| xmac) {
3759	4318	int status = stmmac_host_irq_status(priv, priv->hw, &priv->xstats);
3760		- int mtl_status;
3761	4319
3762	4320	if (unlikely(status)) {
3763	4321	/* For LPI we need to save the tx status */
..	..	@@ -3768,17 +4326,8 @@
3768	4326	}
3769	4327
3770	4328	for (queue = 0; queue < queues_count; queue++) {
3771		- struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
3772		-
3773		- mtl_status = stmmac_host_mtl_irq_status(priv, priv->hw,
3774		- queue);
3775		- if (mtl_status != -EINVAL)
3776		- status \|= mtl_status;
3777		-
3778		- if (status & CORE_IRQ_MTL_RX_OVERFLOW)
3779		- stmmac_set_rx_tail_ptr(priv, priv->ioaddr,
3780		- rx_q->rx_tail_addr,
3781		- queue);
	4329	+ status = stmmac_host_mtl_irq_status(priv, priv->hw,
	4330	+ queue);
3782	4331	}
3783	4332
3784	4333	/* PCS link status */
..	..	@@ -3819,6 +4368,7 @@
3819	4368	*/
3820	4369	static int stmmac_ioctl(struct net_device dev, struct ifreq rq, int cmd)
3821	4370	{
	4371	+ struct stmmac_priv *priv = netdev_priv (dev);
3822	4372	int ret = -EOPNOTSUPP;
3823	4373
3824	4374	if (!netif_running(dev))
..	..	@@ -3828,9 +4378,7 @@
3828	4378	case SIOCGMIIPHY:
3829	4379	case SIOCGMIIREG:
3830	4380	case SIOCSMIIREG:
3831		- if (!dev->phydev)
3832		- return -EINVAL;
3833		- ret = phy_mii_ioctl(dev->phydev, rq, cmd);
	4381	+ ret = phylink_mii_ioctl(priv->phylink, rq, cmd);
3834	4382	break;
3835	4383	case SIOCSHWTSTAMP:
3836	4384	ret = stmmac_hwtstamp_set(dev, rq);
..	..	@@ -3851,12 +4399,17 @@
3851	4399	struct stmmac_priv *priv = cb_priv;
3852	4400	int ret = -EOPNOTSUPP;
3853	4401
	4402	+ if (!tc_cls_can_offload_and_chain0(priv->dev, type_data))
	4403	+ return ret;
	4404	+
3854	4405	stmmac_disable_all_queues(priv);
3855	4406
3856	4407	switch (type) {
3857	4408	case TC_SETUP_CLSU32:
3858		- if (tc_cls_can_offload_and_chain0(priv->dev, type_data))
3859		- ret = stmmac_tc_setup_cls_u32(priv, priv, type_data);
	4409	+ ret = stmmac_tc_setup_cls_u32(priv, priv, type_data);
	4410	+ break;
	4411	+ case TC_SETUP_CLSFLOWER:
	4412	+ ret = stmmac_tc_setup_cls(priv, priv, type_data);
3860	4413	break;
3861	4414	default:
3862	4415	break;
..	..	@@ -3866,23 +4419,7 @@
3866	4419	return ret;
3867	4420	}
3868	4421
3869		-static int stmmac_setup_tc_block(struct stmmac_priv *priv,
3870		- struct tc_block_offload *f)
3871		-{
3872		- if (f->binder_type != TCF_BLOCK_BINDER_TYPE_CLSACT_INGRESS)
3873		- return -EOPNOTSUPP;
3874		-
3875		- switch (f->command) {
3876		- case TC_BLOCK_BIND:
3877		- return tcf_block_cb_register(f->block, stmmac_setup_tc_block_cb,
3878		- priv, priv, f->extack);
3879		- case TC_BLOCK_UNBIND:
3880		- tcf_block_cb_unregister(f->block, stmmac_setup_tc_block_cb, priv);
3881		- return 0;
3882		- default:
3883		- return -EOPNOTSUPP;
3884		- }
3885		-}
	4422	+static LIST_HEAD(stmmac_block_cb_list);
3886	4423
3887	4424	static int stmmac_setup_tc(struct net_device *ndev, enum tc_setup_type type,
3888	4425	void *type_data)
..	..	@@ -3891,17 +4428,23 @@
3891	4428
3892	4429	switch (type) {
3893	4430	case TC_SETUP_BLOCK:
3894		- return stmmac_setup_tc_block(priv, type_data);
	4431	+ return flow_block_cb_setup_simple(type_data,
	4432	+ &stmmac_block_cb_list,
	4433	+ stmmac_setup_tc_block_cb,
	4434	+ priv, priv, true);
3895	4435	case TC_SETUP_QDISC_CBS:
3896	4436	return stmmac_tc_setup_cbs(priv, priv, type_data);
	4437	+ case TC_SETUP_QDISC_TAPRIO:
	4438	+ return stmmac_tc_setup_taprio(priv, priv, type_data);
	4439	+ case TC_SETUP_QDISC_ETF:
	4440	+ return stmmac_tc_setup_etf(priv, priv, type_data);
3897	4441	default:
3898	4442	return -EOPNOTSUPP;
3899	4443	}
3900	4444	}
3901	4445
3902	4446	static u16 stmmac_select_queue(struct net_device dev, struct sk_buff skb,
3903		- struct net_device *sb_dev,
3904		- select_queue_fallback_t fallback)
	4447	+ struct net_device *sb_dev)
3905	4448	{
3906	4449	int gso = skb_shinfo(skb)->gso_type;
3907	4450
..	..	@@ -3915,7 +4458,7 @@
3915	4458	return 0;
3916	4459	}
3917	4460
3918		- return fallback(dev, skb, NULL) % dev->real_num_tx_queues;
	4461	+ return netdev_pick_tx(dev, skb, NULL) % dev->real_num_tx_queues;
3919	4462	}
3920	4463
3921	4464	static int stmmac_set_mac_address(struct net_device ndev, void addr)
..	..	@@ -3923,11 +4466,20 @@
3923	4466	struct stmmac_priv *priv = netdev_priv(ndev);
3924	4467	int ret = 0;
3925	4468
	4469	+ ret = pm_runtime_get_sync(priv->device);
	4470	+ if (ret < 0) {
	4471	+ pm_runtime_put_noidle(priv->device);
	4472	+ return ret;
	4473	+ }
	4474	+
3926	4475	ret = eth_mac_addr(ndev, addr);
3927	4476	if (ret)
3928		- return ret;
	4477	+ goto set_mac_error;
3929	4478
3930	4479	stmmac_set_umac_addr(priv, priv->hw, ndev->dev_addr, 0);
	4480	+
	4481	+set_mac_error:
	4482	+ pm_runtime_put(priv->device);
3931	4483
3932	4484	return ret;
3933	4485	}
..	..	@@ -3936,24 +4488,27 @@
3936	4488	static struct dentry *stmmac_fs_dir;
3937	4489
3938	4490	static void sysfs_display_ring(void *head, int size, int extend_desc,
3939		- struct seq_file *seq)
	4491	+ struct seq_file *seq, dma_addr_t dma_phy_addr)
3940	4492	{
3941	4493	int i;
3942	4494	struct dma_extended_desc ep = (struct dma_extended_desc )head;
3943	4495	struct dma_desc p = (struct dma_desc )head;
	4496	+ dma_addr_t dma_addr;
3944	4497
3945	4498	for (i = 0; i < size; i++) {
3946	4499	if (extend_desc) {
3947		- seq_printf(seq, "%d [0x%x]: 0x%x 0x%x 0x%x 0x%x\n",
3948		- i, (unsigned int)virt_to_phys(ep),
	4500	+ dma_addr = dma_phy_addr + i * sizeof(*ep);
	4501	+ seq_printf(seq, "%d [%pad]: 0x%x 0x%x 0x%x 0x%x\n",
	4502	+ i, &dma_addr,
3949	4503	le32_to_cpu(ep->basic.des0),
3950	4504	le32_to_cpu(ep->basic.des1),
3951	4505	le32_to_cpu(ep->basic.des2),
3952	4506	le32_to_cpu(ep->basic.des3));
3953	4507	ep++;
3954	4508	} else {
3955		- seq_printf(seq, "%d [0x%x]: 0x%x 0x%x 0x%x 0x%x\n",
3956		- i, (unsigned int)virt_to_phys(p),
	4509	+ dma_addr = dma_phy_addr + i * sizeof(*p);
	4510	+ seq_printf(seq, "%d [%pad]: 0x%x 0x%x 0x%x 0x%x\n",
	4511	+ i, &dma_addr,
3957	4512	le32_to_cpu(p->des0), le32_to_cpu(p->des1),
3958	4513	le32_to_cpu(p->des2), le32_to_cpu(p->des3));
3959	4514	p++;
..	..	@@ -3962,7 +4517,7 @@
3962	4517	}
3963	4518	}
3964	4519
3965		-static int stmmac_sysfs_ring_read(struct seq_file seq, void v)
	4520	+static int stmmac_rings_status_show(struct seq_file seq, void v)
3966	4521	{
3967	4522	struct net_device *dev = seq->private;
3968	4523	struct stmmac_priv *priv = netdev_priv(dev);
..	..	@@ -3981,11 +4536,11 @@
3981	4536	if (priv->extend_desc) {
3982	4537	seq_printf(seq, "Extended descriptor ring:\n");
3983	4538	sysfs_display_ring((void *)rx_q->dma_erx,
3984		- DMA_RX_SIZE, 1, seq);
	4539	+ priv->dma_rx_size, 1, seq, rx_q->dma_rx_phy);
3985	4540	} else {
3986	4541	seq_printf(seq, "Descriptor ring:\n");
3987	4542	sysfs_display_ring((void *)rx_q->dma_rx,
3988		- DMA_RX_SIZE, 0, seq);
	4543	+ priv->dma_rx_size, 0, seq, rx_q->dma_rx_phy);
3989	4544	}
3990	4545	}
3991	4546
..	..	@@ -3997,33 +4552,19 @@
3997	4552	if (priv->extend_desc) {
3998	4553	seq_printf(seq, "Extended descriptor ring:\n");
3999	4554	sysfs_display_ring((void *)tx_q->dma_etx,
4000		- DMA_TX_SIZE, 1, seq);
4001		- } else {
	4555	+ priv->dma_tx_size, 1, seq, tx_q->dma_tx_phy);
	4556	+ } else if (!(tx_q->tbs & STMMAC_TBS_AVAIL)) {
4002	4557	seq_printf(seq, "Descriptor ring:\n");
4003	4558	sysfs_display_ring((void *)tx_q->dma_tx,
4004		- DMA_TX_SIZE, 0, seq);
	4559	+ priv->dma_tx_size, 0, seq, tx_q->dma_tx_phy);
4005	4560	}
4006	4561	}
4007	4562
4008	4563	return 0;
4009	4564	}
	4565	+DEFINE_SHOW_ATTRIBUTE(stmmac_rings_status);
4010	4566
4011		-static int stmmac_sysfs_ring_open(struct inode inode, struct file file)
4012		-{
4013		- return single_open(file, stmmac_sysfs_ring_read, inode->i_private);
4014		-}
4015		-
4016		-/* Debugfs files, should appear in /sys/kernel/debug/stmmaceth/eth0 */
4017		-
4018		-static const struct file_operations stmmac_rings_status_fops = {
4019		- .owner = THIS_MODULE,
4020		- .open = stmmac_sysfs_ring_open,
4021		- .read = seq_read,
4022		- .llseek = seq_lseek,
4023		- .release = single_release,
4024		-};
4025		-
4026		-static int stmmac_sysfs_dma_cap_read(struct seq_file seq, void v)
	4567	+static int stmmac_dma_cap_show(struct seq_file seq, void v)
4027	4568	{
4028	4569	struct net_device *dev = seq->private;
4029	4570	struct stmmac_priv *priv = netdev_priv(dev);
..	..	@@ -4081,64 +4622,94 @@
4081	4622	priv->dma_cap.number_rx_channel);
4082	4623	seq_printf(seq, "\tNumber of Additional TX channel: %d\n",
4083	4624	priv->dma_cap.number_tx_channel);
	4625	+ seq_printf(seq, "\tNumber of Additional RX queues: %d\n",
	4626	+ priv->dma_cap.number_rx_queues);
	4627	+ seq_printf(seq, "\tNumber of Additional TX queues: %d\n",
	4628	+ priv->dma_cap.number_tx_queues);
4084	4629	seq_printf(seq, "\tEnhanced descriptors: %s\n",
4085	4630	(priv->dma_cap.enh_desc) ? "Y" : "N");
4086		-
	4631	+ seq_printf(seq, "\tTX Fifo Size: %d\n", priv->dma_cap.tx_fifo_size);
	4632	+ seq_printf(seq, "\tRX Fifo Size: %d\n", priv->dma_cap.rx_fifo_size);
	4633	+ seq_printf(seq, "\tHash Table Size: %d\n", priv->dma_cap.hash_tb_sz);
	4634	+ seq_printf(seq, "\tTSO: %s\n", priv->dma_cap.tsoen ? "Y" : "N");
	4635	+ seq_printf(seq, "\tNumber of PPS Outputs: %d\n",
	4636	+ priv->dma_cap.pps_out_num);
	4637	+ seq_printf(seq, "\tSafety Features: %s\n",
	4638	+ priv->dma_cap.asp ? "Y" : "N");
	4639	+ seq_printf(seq, "\tFlexible RX Parser: %s\n",
	4640	+ priv->dma_cap.frpsel ? "Y" : "N");
	4641	+ seq_printf(seq, "\tEnhanced Addressing: %d\n",
	4642	+ priv->dma_cap.addr64);
	4643	+ seq_printf(seq, "\tReceive Side Scaling: %s\n",
	4644	+ priv->dma_cap.rssen ? "Y" : "N");
	4645	+ seq_printf(seq, "\tVLAN Hash Filtering: %s\n",
	4646	+ priv->dma_cap.vlhash ? "Y" : "N");
	4647	+ seq_printf(seq, "\tSplit Header: %s\n",
	4648	+ priv->dma_cap.sphen ? "Y" : "N");
	4649	+ seq_printf(seq, "\tVLAN TX Insertion: %s\n",
	4650	+ priv->dma_cap.vlins ? "Y" : "N");
	4651	+ seq_printf(seq, "\tDouble VLAN: %s\n",
	4652	+ priv->dma_cap.dvlan ? "Y" : "N");
	4653	+ seq_printf(seq, "\tNumber of L3/L4 Filters: %d\n",
	4654	+ priv->dma_cap.l3l4fnum);
	4655	+ seq_printf(seq, "\tARP Offloading: %s\n",
	4656	+ priv->dma_cap.arpoffsel ? "Y" : "N");
	4657	+ seq_printf(seq, "\tEnhancements to Scheduled Traffic (EST): %s\n",
	4658	+ priv->dma_cap.estsel ? "Y" : "N");
	4659	+ seq_printf(seq, "\tFrame Preemption (FPE): %s\n",
	4660	+ priv->dma_cap.fpesel ? "Y" : "N");
	4661	+ seq_printf(seq, "\tTime-Based Scheduling (TBS): %s\n",
	4662	+ priv->dma_cap.tbssel ? "Y" : "N");
4087	4663	return 0;
4088	4664	}
	4665	+DEFINE_SHOW_ATTRIBUTE(stmmac_dma_cap);
4089	4666
4090		-static int stmmac_sysfs_dma_cap_open(struct inode inode, struct file file)
	4667	+/* Use network device events to rename debugfs file entries.
	4668	+ */
	4669	+static int stmmac_device_event(struct notifier_block *unused,
	4670	+ unsigned long event, void *ptr)
4091	4671	{
4092		- return single_open(file, stmmac_sysfs_dma_cap_read, inode->i_private);
	4672	+ struct net_device *dev = netdev_notifier_info_to_dev(ptr);
	4673	+ struct stmmac_priv *priv = netdev_priv(dev);
	4674	+
	4675	+ if (dev->netdev_ops != &stmmac_netdev_ops)
	4676	+ goto done;
	4677	+
	4678	+ switch (event) {
	4679	+ case NETDEV_CHANGENAME:
	4680	+ if (priv->dbgfs_dir)
	4681	+ priv->dbgfs_dir = debugfs_rename(stmmac_fs_dir,
	4682	+ priv->dbgfs_dir,
	4683	+ stmmac_fs_dir,
	4684	+ dev->name);
	4685	+ break;
	4686	+ }
	4687	+done:
	4688	+ return NOTIFY_DONE;
4093	4689	}
4094	4690
4095		-static const struct file_operations stmmac_dma_cap_fops = {
4096		- .owner = THIS_MODULE,
4097		- .open = stmmac_sysfs_dma_cap_open,
4098		- .read = seq_read,
4099		- .llseek = seq_lseek,
4100		- .release = single_release,
	4691	+static struct notifier_block stmmac_notifier = {
	4692	+ .notifier_call = stmmac_device_event,
4101	4693	};
4102	4694
4103		-static int stmmac_init_fs(struct net_device *dev)
	4695	+static void stmmac_init_fs(struct net_device *dev)
4104	4696	{
4105	4697	struct stmmac_priv *priv = netdev_priv(dev);
	4698	+
	4699	+ rtnl_lock();
4106	4700
4107	4701	/* Create per netdev entries */
4108	4702	priv->dbgfs_dir = debugfs_create_dir(dev->name, stmmac_fs_dir);
4109	4703
4110		- if (!priv->dbgfs_dir \|\| IS_ERR(priv->dbgfs_dir)) {
4111		- netdev_err(priv->dev, "ERROR failed to create debugfs directory\n");
4112		-
4113		- return -ENOMEM;
4114		- }
4115		-
4116	4704	/* Entry to report DMA RX/TX rings */
4117		- priv->dbgfs_rings_status =
4118		- debugfs_create_file("descriptors_status", 0444,
4119		- priv->dbgfs_dir, dev,
4120		- &stmmac_rings_status_fops);
4121		-
4122		- if (!priv->dbgfs_rings_status \|\| IS_ERR(priv->dbgfs_rings_status)) {
4123		- netdev_err(priv->dev, "ERROR creating stmmac ring debugfs file\n");
4124		- debugfs_remove_recursive(priv->dbgfs_dir);
4125		-
4126		- return -ENOMEM;
4127		- }
	4705	+ debugfs_create_file("descriptors_status", 0444, priv->dbgfs_dir, dev,
	4706	+ &stmmac_rings_status_fops);
4128	4707
4129	4708	/* Entry to report the DMA HW features */
4130		- priv->dbgfs_dma_cap = debugfs_create_file("dma_cap", 0444,
4131		- priv->dbgfs_dir,
4132		- dev, &stmmac_dma_cap_fops);
	4709	+ debugfs_create_file("dma_cap", 0444, priv->dbgfs_dir, dev,
	4710	+ &stmmac_dma_cap_fops);
4133	4711
4134		- if (!priv->dbgfs_dma_cap \|\| IS_ERR(priv->dbgfs_dma_cap)) {
4135		- netdev_err(priv->dev, "ERROR creating stmmac MMC debugfs file\n");
4136		- debugfs_remove_recursive(priv->dbgfs_dir);
4137		-
4138		- return -ENOMEM;
4139		- }
4140		-
4141		- return 0;
	4712	+ rtnl_unlock();
4142	4713	}
4143	4714
4144	4715	static void stmmac_exit_fs(struct net_device *dev)
..	..	@@ -4148,6 +4719,111 @@
4148	4719	debugfs_remove_recursive(priv->dbgfs_dir);
4149	4720	}
4150	4721	#endif /* CONFIG_DEBUG_FS */
	4722	+
	4723	+static u32 stmmac_vid_crc32_le(__le16 vid_le)
	4724	+{
	4725	+ unsigned char data = (unsigned char )&vid_le;
	4726	+ unsigned char data_byte = 0;
	4727	+ u32 crc = ~0x0;
	4728	+ u32 temp = 0;
	4729	+ int i, bits;
	4730	+
	4731	+ bits = get_bitmask_order(VLAN_VID_MASK);
	4732	+ for (i = 0; i < bits; i++) {
	4733	+ if ((i % 8) == 0)
	4734	+ data_byte = data[i / 8];
	4735	+
	4736	+ temp = ((crc & 1) ^ data_byte) & 1;
	4737	+ crc >>= 1;
	4738	+ data_byte >>= 1;
	4739	+
	4740	+ if (temp)
	4741	+ crc ^= 0xedb88320;
	4742	+ }
	4743	+
	4744	+ return crc;
	4745	+}
	4746	+
	4747	+static int stmmac_vlan_update(struct stmmac_priv *priv, bool is_double)
	4748	+{
	4749	+ u32 crc, hash = 0;
	4750	+ __le16 pmatch = 0;
	4751	+ int count = 0;
	4752	+ u16 vid = 0;
	4753	+
	4754	+ for_each_set_bit(vid, priv->active_vlans, VLAN_N_VID) {
	4755	+ __le16 vid_le = cpu_to_le16(vid);
	4756	+ crc = bitrev32(~stmmac_vid_crc32_le(vid_le)) >> 28;
	4757	+ hash \|= (1 << crc);
	4758	+ count++;
	4759	+ }
	4760	+
	4761	+ if (!priv->dma_cap.vlhash) {
	4762	+ if (count > 2) /* VID = 0 always passes filter */
	4763	+ return -EOPNOTSUPP;
	4764	+
	4765	+ pmatch = cpu_to_le16(vid);
	4766	+ hash = 0;
	4767	+ }
	4768	+
	4769	+ return stmmac_update_vlan_hash(priv, priv->hw, hash, pmatch, is_double);
	4770	+}
	4771	+
	4772	+static int stmmac_vlan_rx_add_vid(struct net_device *ndev, __be16 proto, u16 vid)
	4773	+{
	4774	+ struct stmmac_priv *priv = netdev_priv(ndev);
	4775	+ bool is_double = false;
	4776	+ int ret;
	4777	+
	4778	+ if (be16_to_cpu(proto) == ETH_P_8021AD)
	4779	+ is_double = true;
	4780	+
	4781	+ set_bit(vid, priv->active_vlans);
	4782	+ ret = stmmac_vlan_update(priv, is_double);
	4783	+ if (ret) {
	4784	+ clear_bit(vid, priv->active_vlans);
	4785	+ return ret;
	4786	+ }
	4787	+
	4788	+ if (priv->hw->num_vlan) {
	4789	+ ret = stmmac_add_hw_vlan_rx_fltr(priv, ndev, priv->hw, proto, vid);
	4790	+ if (ret)
	4791	+ return ret;
	4792	+ }
	4793	+
	4794	+ return 0;
	4795	+}
	4796	+
	4797	+static int stmmac_vlan_rx_kill_vid(struct net_device *ndev, __be16 proto, u16 vid)
	4798	+{
	4799	+ struct stmmac_priv *priv = netdev_priv(ndev);
	4800	+ bool is_double = false;
	4801	+ int ret;
	4802	+
	4803	+ ret = pm_runtime_get_sync(priv->device);
	4804	+ if (ret < 0) {
	4805	+ pm_runtime_put_noidle(priv->device);
	4806	+ return ret;
	4807	+ }
	4808	+
	4809	+ if (be16_to_cpu(proto) == ETH_P_8021AD)
	4810	+ is_double = true;
	4811	+
	4812	+ clear_bit(vid, priv->active_vlans);
	4813	+
	4814	+ if (priv->hw->num_vlan) {
	4815	+ ret = stmmac_del_hw_vlan_rx_fltr(priv, ndev, priv->hw, proto, vid);
	4816	+ if (ret)
	4817	+ goto del_vlan_error;
	4818	+ }
	4819	+
	4820	+ ret = stmmac_vlan_update(priv, is_double);
	4821	+
	4822	+del_vlan_error:
	4823	+ pm_runtime_put(priv->device);
	4824	+
	4825	+ return ret;
	4826	+}
4151	4827
4152	4828	static const struct net_device_ops stmmac_netdev_ops = {
4153	4829	.ndo_open = stmmac_open,
..	..	@@ -4165,6 +4841,8 @@
4165	4841	.ndo_poll_controller = stmmac_poll_controller,
4166	4842	#endif
4167	4843	.ndo_set_mac_address = stmmac_set_mac_address,
	4844	+ .ndo_vlan_rx_add_vid = stmmac_vlan_rx_add_vid,
	4845	+ .ndo_vlan_rx_kill_vid = stmmac_vlan_rx_kill_vid,
4168	4846	};
4169	4847
4170	4848	static void stmmac_reset_subtask(struct stmmac_priv *priv)
..	..	@@ -4183,7 +4861,7 @@
4183	4861
4184	4862	set_bit(STMMAC_DOWN, &priv->state);
4185	4863	dev_close(priv->dev);
4186		- dev_open(priv->dev);
	4864	+ dev_open(priv->dev, NULL);
4187	4865	clear_bit(STMMAC_DOWN, &priv->state);
4188	4866	clear_bit(STMMAC_RESETING, &priv->state);
4189	4867	rtnl_unlock();
..	..	@@ -4233,6 +4911,12 @@
4233	4911	priv->plat->enh_desc = priv->dma_cap.enh_desc;
4234	4912	priv->plat->pmt = priv->dma_cap.pmt_remote_wake_up;
4235	4913	priv->hw->pmt = priv->plat->pmt;
	4914	+ if (priv->dma_cap.hash_tb_sz) {
	4915	+ priv->hw->multicast_filter_bins =
	4916	+ (BIT(priv->dma_cap.hash_tb_sz) << 5);
	4917	+ priv->hw->mcast_bits_log2 =
	4918	+ ilog2(priv->hw->multicast_filter_bins);
	4919	+ }
4236	4920
4237	4921	/* TXCOE doesn't work in thresh DMA mode */
4238	4922	if (priv->plat->force_thresh_dma_mode)
..	..	@@ -4269,6 +4953,9 @@
4269	4953	if (priv->dma_cap.tsoen)
4270	4954	dev_info(priv->device, "TSO supported\n");
4271	4955
	4956	+ priv->hw->vlan_fail_q_en = priv->plat->vlan_fail_q_en;
	4957	+ priv->hw->vlan_fail_q = priv->plat->vlan_fail_q;
	4958	+
4272	4959	/* Run HW quirks, if any */
4273	4960	if (priv->hwif_quirks) {
4274	4961	ret = priv->hwif_quirks(priv);
..	..	@@ -4291,6 +4978,92 @@
4291	4978	return 0;
4292	4979	}
4293	4980
	4981	+static void stmmac_napi_add(struct net_device *dev)
	4982	+{
	4983	+ struct stmmac_priv *priv = netdev_priv(dev);
	4984	+ u32 queue, maxq;
	4985	+
	4986	+ maxq = max(priv->plat->rx_queues_to_use, priv->plat->tx_queues_to_use);
	4987	+
	4988	+ for (queue = 0; queue < maxq; queue++) {
	4989	+ struct stmmac_channel *ch = &priv->channel[queue];
	4990	+ int rx_budget = ((priv->plat->dma_rx_size < NAPI_POLL_WEIGHT) &&
	4991	+ (priv->plat->dma_rx_size > 0)) ?
	4992	+ priv->plat->dma_rx_size : NAPI_POLL_WEIGHT;
	4993	+ int tx_budget = ((priv->plat->dma_tx_size < NAPI_POLL_WEIGHT) &&
	4994	+ (priv->plat->dma_tx_size > 0)) ?
	4995	+ priv->plat->dma_tx_size : NAPI_POLL_WEIGHT;
	4996	+
	4997	+ ch->priv_data = priv;
	4998	+ ch->index = queue;
	4999	+ spin_lock_init(&ch->lock);
	5000	+
	5001	+ if (queue < priv->plat->rx_queues_to_use) {
	5002	+ netif_napi_add(dev, &ch->rx_napi, stmmac_napi_poll_rx,
	5003	+ rx_budget);
	5004	+ }
	5005	+ if (queue < priv->plat->tx_queues_to_use) {
	5006	+ netif_tx_napi_add(dev, &ch->tx_napi,
	5007	+ stmmac_napi_poll_tx, tx_budget);
	5008	+ }
	5009	+ }
	5010	+}
	5011	+
	5012	+static void stmmac_napi_del(struct net_device *dev)
	5013	+{
	5014	+ struct stmmac_priv *priv = netdev_priv(dev);
	5015	+ u32 queue, maxq;
	5016	+
	5017	+ maxq = max(priv->plat->rx_queues_to_use, priv->plat->tx_queues_to_use);
	5018	+
	5019	+ for (queue = 0; queue < maxq; queue++) {
	5020	+ struct stmmac_channel *ch = &priv->channel[queue];
	5021	+
	5022	+ if (queue < priv->plat->rx_queues_to_use)
	5023	+ netif_napi_del(&ch->rx_napi);
	5024	+ if (queue < priv->plat->tx_queues_to_use)
	5025	+ netif_napi_del(&ch->tx_napi);
	5026	+ }
	5027	+}
	5028	+
	5029	+int stmmac_reinit_queues(struct net_device *dev, u32 rx_cnt, u32 tx_cnt)
	5030	+{
	5031	+ struct stmmac_priv *priv = netdev_priv(dev);
	5032	+ int ret = 0;
	5033	+
	5034	+ if (netif_running(dev))
	5035	+ stmmac_release(dev);
	5036	+
	5037	+ stmmac_napi_del(dev);
	5038	+
	5039	+ priv->plat->rx_queues_to_use = rx_cnt;
	5040	+ priv->plat->tx_queues_to_use = tx_cnt;
	5041	+
	5042	+ stmmac_napi_add(dev);
	5043	+
	5044	+ if (netif_running(dev))
	5045	+ ret = stmmac_open(dev);
	5046	+
	5047	+ return ret;
	5048	+}
	5049	+
	5050	+int stmmac_reinit_ringparam(struct net_device *dev, u32 rx_size, u32 tx_size)
	5051	+{
	5052	+ struct stmmac_priv *priv = netdev_priv(dev);
	5053	+ int ret = 0;
	5054	+
	5055	+ if (netif_running(dev))
	5056	+ stmmac_release(dev);
	5057	+
	5058	+ priv->dma_rx_size = rx_size;
	5059	+ priv->dma_tx_size = tx_size;
	5060	+
	5061	+ if (netif_running(dev))
	5062	+ ret = stmmac_open(dev);
	5063	+
	5064	+ return ret;
	5065	+}
	5066	+
4294	5067	/**
4295	5068	* stmmac_dvr_probe
4296	5069	* @device: device pointer
..	..	@@ -4307,12 +5080,11 @@
4307	5080	{
4308	5081	struct net_device *ndev = NULL;
4309	5082	struct stmmac_priv *priv;
4310		- u32 queue, maxq;
4311		- int ret = 0;
	5083	+ u32 rxq;
	5084	+ int i, ret = 0;
4312	5085
4313		- ndev = alloc_etherdev_mqs(sizeof(struct stmmac_priv),
4314		- MTL_MAX_TX_QUEUES,
4315		- MTL_MAX_RX_QUEUES);
	5086	+ ndev = devm_alloc_etherdev_mqs(device, sizeof(struct stmmac_priv),
	5087	+ MTL_MAX_TX_QUEUES, MTL_MAX_RX_QUEUES);
4316	5088	if (!ndev)
4317	5089	return -ENOMEM;
4318	5090
..	..	@@ -4332,7 +5104,7 @@
4332	5104	priv->wol_irq = res->wol_irq;
4333	5105	priv->lpi_irq = res->lpi_irq;
4334	5106
4335		- if (res->mac)
	5107	+ if (!IS_ERR_OR_NULL(res->mac))
4336	5108	memcpy(priv->dev->dev_addr, res->mac, ETH_ALEN);
4337	5109
4338	5110	dev_set_drvdata(device, priv->dev);
..	..	@@ -4344,8 +5116,7 @@
4344	5116	priv->wq = create_singlethread_workqueue("stmmac_wq");
4345	5117	if (!priv->wq) {
4346	5118	dev_err(priv->device, "failed to create workqueue\n");
4347		- ret = -ENOMEM;
4348		- goto error_wq;
	5119	+ return -ENOMEM;
4349	5120	}
4350	5121
4351	5122	INIT_WORK(&priv->service_task, stmmac_service_task);
..	..	@@ -4373,10 +5144,6 @@
4373	5144
4374	5145	stmmac_check_ether_addr(priv);
4375	5146
4376		- /* Configure real RX and TX queues */
4377		- netif_set_real_num_rx_queues(ndev, priv->plat->rx_queues_to_use);
4378		- netif_set_real_num_tx_queues(ndev, priv->plat->tx_queues_to_use);
4379		-
4380	5147	ndev->netdev_ops = &stmmac_netdev_ops;
4381	5148
4382	5149	ndev->hw_features = NETIF_F_SG \| NETIF_F_IP_CSUM \| NETIF_F_IPV6_CSUM \|
..	..	@@ -4394,20 +5161,79 @@
4394	5161	priv->tso = true;
4395	5162	dev_info(priv->device, "TSO feature enabled\n");
4396	5163	}
	5164	+
	5165	+ if (priv->dma_cap.sphen && !priv->plat->sph_disable) {
	5166	+ ndev->hw_features \|= NETIF_F_GRO;
	5167	+ if (!priv->plat->sph_disable) {
	5168	+ priv->sph = true;
	5169	+ dev_info(priv->device, "SPH feature enabled\n");
	5170	+ }
	5171	+ }
	5172	+
	5173	+ /* The current IP register MAC_HW_Feature1[ADDR64] only define
	5174	+ * 32/40/64 bit width, but some SOC support others like i.MX8MP
	5175	+ * support 34 bits but it map to 40 bits width in MAC_HW_Feature1[ADDR64].
	5176	+ * So overwrite dma_cap.addr64 according to HW real design.
	5177	+ */
	5178	+ if (priv->plat->addr64)
	5179	+ priv->dma_cap.addr64 = priv->plat->addr64;
	5180	+
	5181	+ if (priv->dma_cap.addr64) {
	5182	+ ret = dma_set_mask_and_coherent(device,
	5183	+ DMA_BIT_MASK(priv->dma_cap.addr64));
	5184	+ if (!ret) {
	5185	+ dev_info(priv->device, "Using %d bits DMA width\n",
	5186	+ priv->dma_cap.addr64);
	5187	+
	5188	+ /*
	5189	+ * If more than 32 bits can be addressed, make sure to
	5190	+ * enable enhanced addressing mode.
	5191	+ */
	5192	+ if (IS_ENABLED(CONFIG_ARCH_DMA_ADDR_T_64BIT))
	5193	+ priv->plat->dma_cfg->eame = true;
	5194	+ } else {
	5195	+ ret = dma_set_mask_and_coherent(device, DMA_BIT_MASK(32));
	5196	+ if (ret) {
	5197	+ dev_err(priv->device, "Failed to set DMA Mask\n");
	5198	+ goto error_hw_init;
	5199	+ }
	5200	+
	5201	+ priv->dma_cap.addr64 = 32;
	5202	+ }
	5203	+ }
	5204	+
4397	5205	ndev->features \|= ndev->hw_features \| NETIF_F_HIGHDMA;
4398	5206	ndev->watchdog_timeo = msecs_to_jiffies(watchdog);
4399	5207	#ifdef STMMAC_VLAN_TAG_USED
4400	5208	/* Both mac100 and gmac support receive VLAN tag detection */
4401	5209	ndev->features \|= NETIF_F_HW_VLAN_CTAG_RX \| NETIF_F_HW_VLAN_STAG_RX;
	5210	+ if (priv->dma_cap.vlhash) {
	5211	+ ndev->features \|= NETIF_F_HW_VLAN_CTAG_FILTER;
	5212	+ ndev->features \|= NETIF_F_HW_VLAN_STAG_FILTER;
	5213	+ }
	5214	+ if (priv->dma_cap.vlins) {
	5215	+ ndev->features \|= NETIF_F_HW_VLAN_CTAG_TX;
	5216	+ if (priv->dma_cap.dvlan)
	5217	+ ndev->features \|= NETIF_F_HW_VLAN_STAG_TX;
	5218	+ }
4402	5219	#endif
4403	5220	priv->msg_enable = netif_msg_init(debug, default_msg_level);
4404	5221
	5222	+ /* Initialize RSS */
	5223	+ rxq = priv->plat->rx_queues_to_use;
	5224	+ netdev_rss_key_fill(priv->rss.key, sizeof(priv->rss.key));
	5225	+ for (i = 0; i < ARRAY_SIZE(priv->rss.table); i++)
	5226	+ priv->rss.table[i] = ethtool_rxfh_indir_default(i, rxq);
	5227	+
	5228	+ if (priv->dma_cap.rssen && priv->plat->rss_en)
	5229	+ ndev->features \|= NETIF_F_RXHASH;
	5230	+
4405	5231	/* MTU range: 46 - hw-specific max */
4406	5232	ndev->min_mtu = ETH_ZLEN - ETH_HLEN;
4407		- if ((priv->plat->enh_desc) \|\| (priv->synopsys_id >= DWMAC_CORE_4_00))
4408		- ndev->max_mtu = JUMBO_LEN;
4409		- else if (priv->plat->has_xgmac)
	5233	+ if (priv->plat->has_xgmac)
4410	5234	ndev->max_mtu = XGMAC_JUMBO_LEN;
	5235	+ else if ((priv->plat->enh_desc) \|\| (priv->synopsys_id >= DWMAC_CORE_4_00))
	5236	+ ndev->max_mtu = JUMBO_LEN;
4411	5237	else
4412	5238	ndev->max_mtu = SKB_MAX_HEAD(NET_SKB_PAD + NET_IP_ALIGN);
4413	5239	/* Will not overwrite ndev->max_mtu if plat->maxmtu > ndev->max_mtu
..	..	@@ -4425,22 +5251,7 @@
4425	5251	priv->flow_ctrl = FLOW_AUTO; /* RX/TX pause on */
4426	5252
4427	5253	/* Setup channels NAPI */
4428		- maxq = max(priv->plat->rx_queues_to_use, priv->plat->tx_queues_to_use);
4429		-
4430		- for (queue = 0; queue < maxq; queue++) {
4431		- struct stmmac_channel *ch = &priv->channel[queue];
4432		-
4433		- ch->priv_data = priv;
4434		- ch->index = queue;
4435		-
4436		- if (queue < priv->plat->rx_queues_to_use)
4437		- ch->has_rx = true;
4438		- if (queue < priv->plat->tx_queues_to_use)
4439		- ch->has_tx = true;
4440		-
4441		- netif_napi_add(ndev, &ch->napi, stmmac_napi_poll,
4442		- NAPI_POLL_WEIGHT);
4443		- }
	5254	+ stmmac_napi_add(ndev);
4444	5255
4445	5256	mutex_init(&priv->lock);
4446	5257
..	..	@@ -4450,15 +5261,18 @@
4450	5261	* set the MDC clock dynamically according to the csr actual
4451	5262	* clock input.
4452	5263	*/
4453		- if (!priv->plat->clk_csr)
4454		- stmmac_clk_csr_set(priv);
4455		- else
	5264	+ if (priv->plat->clk_csr >= 0)
4456	5265	priv->clk_csr = priv->plat->clk_csr;
	5266	+ else
	5267	+ stmmac_clk_csr_set(priv);
4457	5268
4458	5269	stmmac_check_pcs_mode(priv);
4459	5270
4460		- if (priv->hw->pcs != STMMAC_PCS_RGMII &&
4461		- priv->hw->pcs != STMMAC_PCS_TBI &&
	5271	+ pm_runtime_get_noresume(device);
	5272	+ pm_runtime_set_active(device);
	5273	+ pm_runtime_enable(device);
	5274	+
	5275	+ if (priv->hw->pcs != STMMAC_PCS_TBI &&
4462	5276	priv->hw->pcs != STMMAC_PCS_RTBI) {
4463	5277	/* MDIO bus Registration */
4464	5278	ret = stmmac_mdio_register(ndev);
..	..	@@ -4470,6 +5284,12 @@
4470	5284	}
4471	5285	}
4472	5286
	5287	+ ret = stmmac_phy_setup(priv);
	5288	+ if (ret) {
	5289	+ netdev_err(ndev, "failed to setup phy (%d)\n", ret);
	5290	+ goto error_phy_setup;
	5291	+ }
	5292	+
4473	5293	ret = register_netdev(ndev);
4474	5294	if (ret) {
4475	5295	dev_err(priv->device, "%s: ERROR %i registering the device\n",
..	..	@@ -4478,29 +5298,29 @@
4478	5298	}
4479	5299
4480	5300	#ifdef CONFIG_DEBUG_FS
4481		- ret = stmmac_init_fs(ndev);
4482		- if (ret < 0)
4483		- netdev_warn(priv->dev, "%s: failed debugFS registration\n",
4484		- __func__);
	5301	+ stmmac_init_fs(ndev);
4485	5302	#endif
	5303	+
	5304	+ /* Let pm_runtime_put() disable the clocks.
	5305	+ * If CONFIG_PM is not enabled, the clocks will stay powered.
	5306	+ */
	5307	+ pm_runtime_put(device);
	5308	+
	5309	+ //add
	5310	+ phy_register_fixup_for_uid(RTL_8211F_PHY_ID, RTL_8211F_PHY_ID_MASK, rtl8211F_led_control);
4486	5311
4487	5312	return ret;
4488	5313
4489	5314	error_netdev_register:
4490		- if (priv->hw->pcs != STMMAC_PCS_RGMII &&
4491		- priv->hw->pcs != STMMAC_PCS_TBI &&
	5315	+ phylink_destroy(priv->phylink);
	5316	+error_phy_setup:
	5317	+ if (priv->hw->pcs != STMMAC_PCS_TBI &&
4492	5318	priv->hw->pcs != STMMAC_PCS_RTBI)
4493	5319	stmmac_mdio_unregister(ndev);
4494	5320	error_mdio_register:
4495		- for (queue = 0; queue < maxq; queue++) {
4496		- struct stmmac_channel *ch = &priv->channel[queue];
4497		-
4498		- netif_napi_del(&ch->napi);
4499		- }
	5321	+ stmmac_napi_del(ndev);
4500	5322	error_hw_init:
4501	5323	destroy_workqueue(priv->wq);
4502		-error_wq:
4503		- free_netdev(ndev);
4504	5324
4505	5325	return ret;
4506	5326	}
..	..	@@ -4519,25 +5339,30 @@
4519	5339
4520	5340	netdev_info(priv->dev, "%s: removing driver", __func__);
4521	5341
4522		-#ifdef CONFIG_DEBUG_FS
4523		- stmmac_exit_fs(ndev);
4524		-#endif
4525	5342	stmmac_stop_all_dma(priv);
4526		-
4527	5343	stmmac_mac_set(priv, priv->ioaddr, false);
4528	5344	netif_carrier_off(ndev);
4529	5345	unregister_netdev(ndev);
	5346	+
	5347	+ /* Serdes power down needs to happen after VLAN filter
	5348	+ * is deleted that is triggered by unregister_netdev().
	5349	+ */
	5350	+ if (priv->plat->serdes_powerdown)
	5351	+ priv->plat->serdes_powerdown(ndev, priv->plat->bsp_priv);
	5352	+
	5353	+#ifdef CONFIG_DEBUG_FS
	5354	+ stmmac_exit_fs(ndev);
	5355	+#endif
	5356	+ phylink_destroy(priv->phylink);
4530	5357	if (priv->plat->stmmac_rst)
4531	5358	reset_control_assert(priv->plat->stmmac_rst);
4532		- clk_disable_unprepare(priv->plat->pclk);
4533		- clk_disable_unprepare(priv->plat->stmmac_clk);
4534		- if (priv->hw->pcs != STMMAC_PCS_RGMII &&
4535		- priv->hw->pcs != STMMAC_PCS_TBI &&
	5359	+ pm_runtime_put(dev);
	5360	+ pm_runtime_disable(dev);
	5361	+ if (priv->hw->pcs != STMMAC_PCS_TBI &&
4536	5362	priv->hw->pcs != STMMAC_PCS_RTBI)
4537	5363	stmmac_mdio_unregister(ndev);
4538	5364	destroy_workqueue(priv->wq);
4539	5365	mutex_destroy(&priv->lock);
4540		- free_netdev(ndev);
4541	5366
4542	5367	return 0;
4543	5368	}
..	..	@@ -4559,8 +5384,7 @@
4559	5384	if (!ndev \|\| !netif_running(ndev))
4560	5385	return 0;
4561	5386
4562		- if (ndev->phydev)
4563		- phy_stop(ndev->phydev);
	5387	+ phylink_mac_change(priv->phylink, false);
4564	5388
4565	5389	mutex_lock(&priv->lock);
4566	5390
..	..	@@ -4579,31 +5403,38 @@
4579	5403	/* Stop TX/RX DMA */
4580	5404	stmmac_stop_all_dma(priv);
4581	5405
	5406	+ if (priv->plat->serdes_powerdown)
	5407	+ priv->plat->serdes_powerdown(ndev, priv->plat->bsp_priv);
	5408	+
4582	5409	/* Enable Power down mode by programming the PMT regs */
4583		- if (device_may_wakeup(priv->device)) {
	5410	+ if (device_may_wakeup(priv->device) && priv->plat->pmt) {
4584	5411	stmmac_pmt(priv, priv->hw, priv->wolopts);
4585	5412	priv->irq_wake = 1;
4586	5413	} else {
	5414	+ mutex_unlock(&priv->lock);
	5415	+ rtnl_lock();
	5416	+ if (device_may_wakeup(priv->device))
	5417	+ phylink_speed_down(priv->phylink, false);
	5418	+ if (priv->plat->integrated_phy_power)
	5419	+ priv->plat->integrated_phy_power(priv->plat->bsp_priv,
	5420	+ false);
	5421	+ phylink_stop(priv->phylink);
	5422	+ rtnl_unlock();
	5423	+ mutex_lock(&priv->lock);
	5424	+
4587	5425	stmmac_mac_set(priv, priv->ioaddr, false);
4588	5426	pinctrl_pm_select_sleep_state(priv->device);
4589		- /* Disable clock in case of PWM is off */
4590		- if (priv->plat->clk_ptp_ref)
4591		- clk_disable_unprepare(priv->plat->clk_ptp_ref);
4592		- clk_disable_unprepare(priv->plat->pclk);
4593		- clk_disable_unprepare(priv->plat->stmmac_clk);
4594	5427	}
4595	5428	mutex_unlock(&priv->lock);
4596	5429
4597		- priv->oldlink = false;
4598	5430	priv->speed = SPEED_UNKNOWN;
4599		- priv->oldduplex = DUPLEX_UNKNOWN;
4600	5431	return 0;
4601	5432	}
4602	5433	EXPORT_SYMBOL_GPL(stmmac_suspend);
4603	5434
4604	5435	/**
4605	5436	* stmmac_reset_queues_param - reset queue parameters
4606		- * @dev: device pointer
	5437	+ * @priv: device pointer
4607	5438	*/
4608	5439	static void stmmac_reset_queues_param(struct stmmac_priv *priv)
4609	5440	{
..	..	@@ -4624,6 +5455,8 @@
4624	5455	tx_q->cur_tx = 0;
4625	5456	tx_q->dirty_tx = 0;
4626	5457	tx_q->mss = 0;
	5458	+
	5459	+ netdev_tx_reset_queue(netdev_get_tx_queue(priv->dev, queue));
4627	5460	}
4628	5461	}
4629	5462
..	..	@@ -4637,34 +5470,49 @@
4637	5470	{
4638	5471	struct net_device *ndev = dev_get_drvdata(dev);
4639	5472	struct stmmac_priv *priv = netdev_priv(ndev);
	5473	+ int ret;
4640	5474
4641	5475	if (!netif_running(ndev))
4642	5476	return 0;
4643	5477
4644		- printk("troy test %s start .... \n",__func__);
4645	5478	/* Power Down bit, into the PM register, is cleared
4646	5479	* automatically as soon as a magic packet or a Wake-up frame
4647	5480	* is received. Anyway, it's better to manually clear
4648	5481	* this bit because it can generate problems while resuming
4649	5482	* from another devices (e.g. serial console).
4650	5483	*/
4651		- if (device_may_wakeup(priv->device)) {
	5484	+ if (device_may_wakeup(priv->device) && priv->plat->pmt) {
4652	5485	mutex_lock(&priv->lock);
4653	5486	stmmac_pmt(priv, priv->hw, 0);
4654	5487	mutex_unlock(&priv->lock);
4655	5488	priv->irq_wake = 0;
4656	5489	} else {
4657	5490	pinctrl_pm_select_default_state(priv->device);
4658		- /* enable the clk previously disabled */
4659		- clk_prepare_enable(priv->plat->stmmac_clk);
4660		- clk_prepare_enable(priv->plat->pclk);
4661		- if (priv->plat->clk_ptp_ref)
4662		- clk_prepare_enable(priv->plat->clk_ptp_ref);
4663	5491	/* reset the phy so that it's ready */
4664	5492	if (priv->mii)
4665	5493	stmmac_mdio_reset(priv->mii);
	5494	+ if (priv->plat->integrated_phy_power)
	5495	+ priv->plat->integrated_phy_power(priv->plat->bsp_priv,
	5496	+ true);
4666	5497	}
4667	5498
	5499	+ if (priv->plat->serdes_powerup) {
	5500	+ ret = priv->plat->serdes_powerup(ndev,
	5501	+ priv->plat->bsp_priv);
	5502	+
	5503	+ if (ret < 0)
	5504	+ return ret;
	5505	+ }
	5506	+
	5507	+ if (!device_may_wakeup(priv->device) \|\| !priv->plat->pmt) {
	5508	+ rtnl_lock();
	5509	+ phylink_start(priv->phylink);
	5510	+ /* We may have called phylink_speed_down before */
	5511	+ phylink_speed_up(priv->phylink);
	5512	+ rtnl_unlock();
	5513	+ }
	5514	+
	5515	+ rtnl_lock();
4668	5516	mutex_lock(&priv->lock);
4669	5517
4670	5518	stmmac_reset_queues_param(priv);
..	..	@@ -4672,20 +5520,26 @@
4672	5520	stmmac_free_tx_skbufs(priv);
4673	5521	stmmac_clear_descriptors(priv);
4674	5522
	5523	+#if 1
	5524	+ printk("ben -------resume add 2s delay time.\n");
	5525	+ mdelay(2000);
	5526	+
	5527	+#endif
	5528	+
4675	5529	stmmac_hw_setup(ndev, false);
4676		- stmmac_init_tx_coalesce(priv);
	5530	+ stmmac_init_coalesce(priv);
4677	5531	stmmac_set_rx_mode(ndev);
	5532	+
	5533	+ stmmac_restore_hw_vlan_rx_fltr(priv, ndev, priv->hw);
4678	5534
4679	5535	stmmac_enable_all_queues(priv);
4680	5536
4681		- netif_device_attach(ndev);
4682		-
4683	5537	mutex_unlock(&priv->lock);
	5538	+ rtnl_unlock();
4684	5539
4685		- if (ndev->phydev)
4686		- phy_start(ndev->phydev);
4687		- printk("troy test %s end .... \n",__func__);
4688		- rtl8211F_led_control(ndev->phydev);
	5540	+ phylink_mac_change(priv->phylink, true);
	5541	+
	5542	+ netif_device_attach(ndev);
4689	5543
4690	5544	return 0;
4691	5545	}
..	..	@@ -4697,7 +5551,7 @@
4697	5551	char *opt;
4698	5552
4699	5553	if (!str \|\| !*str)
4700		- return -EINVAL;
	5554	+ return 1;
4701	5555	while ((opt = strsep(&str, ",")) != NULL) {
4702	5556	if (!strncmp(opt, "debug:", 6)) {
4703	5557	if (kstrtoint(opt + 6, 0, &debug))
..	..	@@ -4728,11 +5582,11 @@
4728	5582	goto err;
4729	5583	}
4730	5584	}
4731		- return 0;
	5585	+ return 1;
4732	5586
4733	5587	err:
4734	5588	pr_err("%s: ERROR broken module parameter conversion", __func__);
4735		- return -EINVAL;
	5589	+ return 1;
4736	5590	}
4737	5591
4738	5592	__setup("stmmaceth=", stmmac_cmdline_opt);
..	..	@@ -4742,16 +5596,9 @@
4742	5596	{
4743	5597	#ifdef CONFIG_DEBUG_FS
4744	5598	/* Create debugfs main directory if it doesn't exist yet */
4745		- if (!stmmac_fs_dir) {
	5599	+ if (!stmmac_fs_dir)
4746	5600	stmmac_fs_dir = debugfs_create_dir(STMMAC_RESOURCE_NAME, NULL);
4747		-
4748		- if (!stmmac_fs_dir \|\| IS_ERR(stmmac_fs_dir)) {
4749		- pr_err("ERROR %s, debugfs create directory failed\n",
4750		- STMMAC_RESOURCE_NAME);
4751		-
4752		- return -ENOMEM;
4753		- }
4754		- }
	5601	+ register_netdevice_notifier(&stmmac_notifier);
4755	5602	#endif
4756	5603
4757	5604	return 0;
..	..	@@ -4760,6 +5607,7 @@
4760	5607	static void __exit stmmac_exit(void)
4761	5608	{
4762	5609	#ifdef CONFIG_DEBUG_FS
	5610	+ unregister_netdevice_notifier(&stmmac_notifier);
4763	5611	debugfs_remove_recursive(stmmac_fs_dir);
4764	5612	#endif
4765	5613	}