From: Hauke Mehrtens Date: Thu, 21 Sep 2017 20:10:08 +0000 (+0200) Subject: sunxi: backport the stmmac driver from kernel 4.13 X-Git-Url: https://git.librecmc.org/?a=commitdiff_plain;h=ed43a4d4ac195bf3c149805094b628a0d45f8880;p=librecmc%2Flibrecmc.git sunxi: backport the stmmac driver from kernel 4.13 This adds support for the GMAC which is use in the A64 and other Allwinner chips by backporting the changes from the kernel versions 4.13. Some commits are not backported which are adding support for newly introduced APIs which are not available in kernel 4.9. Signed-off-by: Hauke Mehrtens --- diff --git a/target/linux/sunxi/config-4.9 b/target/linux/sunxi/config-4.9 index 45677f9a83..e961c8ad82 100644 --- a/target/linux/sunxi/config-4.9 +++ b/target/linux/sunxi/config-4.9 @@ -146,7 +146,9 @@ CONFIG_DMA_VIRTUAL_CHANNELS=y CONFIG_DNOTIFY=y CONFIG_DTC=y CONFIG_DUMMY_CONSOLE=y +# CONFIG_DWMAC_DWC_QOS_ETH is not set CONFIG_DWMAC_GENERIC=y +# CONFIG_DWMAC_SUN8I is not set CONFIG_DWMAC_SUNXI=y CONFIG_DYNAMIC_DEBUG=y CONFIG_EDAC_ATOMIC_SCRUB=y diff --git a/target/linux/sunxi/cortexa53/config-default b/target/linux/sunxi/cortexa53/config-default index 58ac214695..527a6f6979 100644 --- a/target/linux/sunxi/cortexa53/config-default +++ b/target/linux/sunxi/cortexa53/config-default @@ -43,6 +43,7 @@ CONFIG_COMMON_CLK_XGENE=y # CONFIG_COMPAT is not set # CONFIG_DEBUG_ALIGN_RODATA is not set CONFIG_DEBUG_RODATA=y +CONFIG_DWMAC_SUN8I=y CONFIG_FRAME_POINTER=y # CONFIG_FSL_ERRATUM_A008585 is not set CONFIG_GENERIC_BUG_RELATIVE_POINTERS=y diff --git a/target/linux/sunxi/cortexa7/config-default b/target/linux/sunxi/cortexa7/config-default index aabfa5ab10..fe974c1033 100644 --- a/target/linux/sunxi/cortexa7/config-default +++ b/target/linux/sunxi/cortexa7/config-default @@ -1,3 +1,4 @@ +CONFIG_DWMAC_SUN8I=y # CONFIG_MACH_SUN4I is not set # CONFIG_MACH_SUN5I is not set # CONFIG_PINCTRL_GR8 is not set diff --git a/target/linux/sunxi/patches-4.9/0050-stmmac-form-4-10.patch b/target/linux/sunxi/patches-4.9/0050-stmmac-form-4-10.patch new file mode 100644 index 0000000000..b88c19e256 --- /dev/null +++ b/target/linux/sunxi/patches-4.9/0050-stmmac-form-4-10.patch @@ -0,0 +1,3497 @@ +--- a/Documentation/devicetree/bindings/net/stmmac.txt ++++ b/Documentation/devicetree/bindings/net/stmmac.txt +@@ -1,7 +1,7 @@ + * STMicroelectronics 10/100/1000 Ethernet driver (GMAC) + + Required properties: +-- compatible: Should be "snps,dwmac-" "snps,dwmac" ++- compatible: Should be "snps,dwmac-", "snps,dwmac" + For backwards compatibility: "st,spear600-gmac" is also supported. + - reg: Address and length of the register set for the device + - interrupt-parent: Should be the phandle for the interrupt controller +@@ -34,7 +34,13 @@ Optional properties: + platforms. + - tx-fifo-depth: See ethernet.txt file in the same directory + - rx-fifo-depth: See ethernet.txt file in the same directory +-- snps,pbl Programmable Burst Length ++- snps,pbl Programmable Burst Length (tx and rx) ++- snps,txpbl Tx Programmable Burst Length. Only for GMAC and newer. ++ If set, DMA tx will use this value rather than snps,pbl. ++- snps,rxpbl Rx Programmable Burst Length. Only for GMAC and newer. ++ If set, DMA rx will use this value rather than snps,pbl. ++- snps,no-pbl-x8 Don't multiply the pbl/txpbl/rxpbl values by 8. ++ For core rev < 3.50, don't multiply the values by 4. + - snps,aal Address-Aligned Beats + - snps,fixed-burst Program the DMA to use the fixed burst mode + - snps,mixed-burst Program the DMA to use the mixed burst mode +@@ -50,6 +56,8 @@ Optional properties: + - snps,ps-speed: port selection speed that can be passed to the core when + PCS is supported. For example, this is used in case of SGMII + and MAC2MAC connection. ++- snps,tso: this enables the TSO feature otherwise it will be managed by ++ MAC HW capability register. Only for GMAC4 and newer. + - AXI BUS Mode parameters: below the list of all the parameters to program the + AXI register inside the DMA module: + - snps,lpi_en: enable Low Power Interface +@@ -62,8 +70,6 @@ Optional properties: + - snps,fb: fixed-burst + - snps,mb: mixed-burst + - snps,rb: rebuild INCRx Burst +- - snps,tso: this enables the TSO feature otherwise it will be managed by +- MAC HW capability register. + - mdio: with compatible = "snps,dwmac-mdio", create and register mdio bus. + + Examples: +--- a/drivers/net/ethernet/stmicro/stmmac/Kconfig ++++ b/drivers/net/ethernet/stmicro/stmmac/Kconfig +@@ -69,6 +69,17 @@ config DWMAC_MESON + the stmmac device driver. This driver is used for Meson6, + Meson8, Meson8b and GXBB SoCs. + ++config DWMAC_OXNAS ++ tristate "Oxford Semiconductor OXNAS dwmac support" ++ default ARCH_OXNAS ++ depends on OF && COMMON_CLK && (ARCH_OXNAS || COMPILE_TEST) ++ select MFD_SYSCON ++ help ++ Support for Ethernet controller on Oxford Semiconductor OXNAS SoCs. ++ ++ This selects the Oxford Semiconductor OXNASSoC glue layer support for ++ the stmmac device driver. This driver is used for OX820. ++ + config DWMAC_ROCKCHIP + tristate "Rockchip dwmac support" + default ARCH_ROCKCHIP +--- a/drivers/net/ethernet/stmicro/stmmac/Makefile ++++ b/drivers/net/ethernet/stmicro/stmmac/Makefile +@@ -10,6 +10,7 @@ obj-$(CONFIG_STMMAC_PLATFORM) += stmmac- + obj-$(CONFIG_DWMAC_IPQ806X) += dwmac-ipq806x.o + obj-$(CONFIG_DWMAC_LPC18XX) += dwmac-lpc18xx.o + obj-$(CONFIG_DWMAC_MESON) += dwmac-meson.o dwmac-meson8b.o ++obj-$(CONFIG_DWMAC_OXNAS) += dwmac-oxnas.o + obj-$(CONFIG_DWMAC_ROCKCHIP) += dwmac-rk.o + obj-$(CONFIG_DWMAC_SOCFPGA) += dwmac-altr-socfpga.o + obj-$(CONFIG_DWMAC_STI) += dwmac-sti.o +--- a/drivers/net/ethernet/stmicro/stmmac/chain_mode.c ++++ b/drivers/net/ethernet/stmicro/stmmac/chain_mode.c +@@ -34,7 +34,7 @@ static int stmmac_jumbo_frm(void *p, str + unsigned int entry = priv->cur_tx; + struct dma_desc *desc = priv->dma_tx + entry; + unsigned int nopaged_len = skb_headlen(skb); +- unsigned int bmax; ++ unsigned int bmax, des2; + unsigned int i = 1, len; + + if (priv->plat->enh_desc) +@@ -44,11 +44,12 @@ static int stmmac_jumbo_frm(void *p, str + + len = nopaged_len - bmax; + +- desc->des2 = dma_map_single(priv->device, skb->data, +- bmax, DMA_TO_DEVICE); +- if (dma_mapping_error(priv->device, desc->des2)) ++ des2 = dma_map_single(priv->device, skb->data, ++ bmax, DMA_TO_DEVICE); ++ desc->des2 = cpu_to_le32(des2); ++ if (dma_mapping_error(priv->device, des2)) + return -1; +- priv->tx_skbuff_dma[entry].buf = desc->des2; ++ priv->tx_skbuff_dma[entry].buf = des2; + priv->tx_skbuff_dma[entry].len = bmax; + /* do not close the descriptor and do not set own bit */ + priv->hw->desc->prepare_tx_desc(desc, 1, bmax, csum, STMMAC_CHAIN_MODE, +@@ -60,12 +61,13 @@ static int stmmac_jumbo_frm(void *p, str + desc = priv->dma_tx + entry; + + if (len > bmax) { +- desc->des2 = dma_map_single(priv->device, +- (skb->data + bmax * i), +- bmax, DMA_TO_DEVICE); +- if (dma_mapping_error(priv->device, desc->des2)) ++ des2 = dma_map_single(priv->device, ++ (skb->data + bmax * i), ++ bmax, DMA_TO_DEVICE); ++ desc->des2 = cpu_to_le32(des2); ++ if (dma_mapping_error(priv->device, des2)) + return -1; +- priv->tx_skbuff_dma[entry].buf = desc->des2; ++ priv->tx_skbuff_dma[entry].buf = des2; + priv->tx_skbuff_dma[entry].len = bmax; + priv->hw->desc->prepare_tx_desc(desc, 0, bmax, csum, + STMMAC_CHAIN_MODE, 1, +@@ -73,12 +75,13 @@ static int stmmac_jumbo_frm(void *p, str + len -= bmax; + i++; + } else { +- desc->des2 = dma_map_single(priv->device, +- (skb->data + bmax * i), len, +- DMA_TO_DEVICE); +- if (dma_mapping_error(priv->device, desc->des2)) ++ des2 = dma_map_single(priv->device, ++ (skb->data + bmax * i), len, ++ DMA_TO_DEVICE); ++ desc->des2 = cpu_to_le32(des2); ++ if (dma_mapping_error(priv->device, des2)) + return -1; +- priv->tx_skbuff_dma[entry].buf = desc->des2; ++ priv->tx_skbuff_dma[entry].buf = des2; + priv->tx_skbuff_dma[entry].len = len; + /* last descriptor can be set now */ + priv->hw->desc->prepare_tx_desc(desc, 0, len, csum, +@@ -119,19 +122,19 @@ static void stmmac_init_dma_chain(void * + struct dma_extended_desc *p = (struct dma_extended_desc *)des; + for (i = 0; i < (size - 1); i++) { + dma_phy += sizeof(struct dma_extended_desc); +- p->basic.des3 = (unsigned int)dma_phy; ++ p->basic.des3 = cpu_to_le32((unsigned int)dma_phy); + p++; + } +- p->basic.des3 = (unsigned int)phy_addr; ++ p->basic.des3 = cpu_to_le32((unsigned int)phy_addr); + + } else { + struct dma_desc *p = (struct dma_desc *)des; + for (i = 0; i < (size - 1); i++) { + dma_phy += sizeof(struct dma_desc); +- p->des3 = (unsigned int)dma_phy; ++ p->des3 = cpu_to_le32((unsigned int)dma_phy); + p++; + } +- p->des3 = (unsigned int)phy_addr; ++ p->des3 = cpu_to_le32((unsigned int)phy_addr); + } + } + +@@ -144,10 +147,10 @@ static void stmmac_refill_desc3(void *pr + * 1588-2002 time stamping is enabled, hence reinitialize it + * to keep explicit chaining in the descriptor. + */ +- p->des3 = (unsigned int)(priv->dma_rx_phy + +- (((priv->dirty_rx) + 1) % +- DMA_RX_SIZE) * +- sizeof(struct dma_desc)); ++ p->des3 = cpu_to_le32((unsigned int)(priv->dma_rx_phy + ++ (((priv->dirty_rx) + 1) % ++ DMA_RX_SIZE) * ++ sizeof(struct dma_desc))); + } + + static void stmmac_clean_desc3(void *priv_ptr, struct dma_desc *p) +@@ -161,9 +164,9 @@ static void stmmac_clean_desc3(void *pri + * 1588-2002 time stamping is enabled, hence reinitialize it + * to keep explicit chaining in the descriptor. + */ +- p->des3 = (unsigned int)((priv->dma_tx_phy + +- ((priv->dirty_tx + 1) % DMA_TX_SIZE)) +- * sizeof(struct dma_desc)); ++ p->des3 = cpu_to_le32((unsigned int)((priv->dma_tx_phy + ++ ((priv->dirty_tx + 1) % DMA_TX_SIZE)) ++ * sizeof(struct dma_desc))); + } + + const struct stmmac_mode_ops chain_mode_ops = { +--- a/drivers/net/ethernet/stmicro/stmmac/common.h ++++ b/drivers/net/ethernet/stmicro/stmmac/common.h +@@ -44,6 +44,7 @@ + #define DWMAC_CORE_4_00 0x40 + #define STMMAC_CHAN0 0 /* Always supported and default for all chips */ + ++/* These need to be power of two, and >= 4 */ + #define DMA_TX_SIZE 512 + #define DMA_RX_SIZE 512 + #define STMMAC_GET_ENTRY(x, size) ((x + 1) & (size - 1)) +@@ -411,8 +412,8 @@ extern const struct stmmac_desc_ops ndes + struct stmmac_dma_ops { + /* DMA core initialization */ + int (*reset)(void __iomem *ioaddr); +- void (*init)(void __iomem *ioaddr, int pbl, int fb, int mb, +- int aal, u32 dma_tx, u32 dma_rx, int atds); ++ void (*init)(void __iomem *ioaddr, struct stmmac_dma_cfg *dma_cfg, ++ u32 dma_tx, u32 dma_rx, int atds); + /* Configure the AXI Bus Mode Register */ + void (*axi)(void __iomem *ioaddr, struct stmmac_axi *axi); + /* Dump DMA registers */ +@@ -506,6 +507,12 @@ struct mac_link { + struct mii_regs { + unsigned int addr; /* MII Address */ + unsigned int data; /* MII Data */ ++ unsigned int addr_shift; /* MII address shift */ ++ unsigned int reg_shift; /* MII reg shift */ ++ unsigned int addr_mask; /* MII address mask */ ++ unsigned int reg_mask; /* MII reg mask */ ++ unsigned int clk_csr_shift; ++ unsigned int clk_csr_mask; + }; + + /* Helpers to manage the descriptors for chain and ring modes */ +--- a/drivers/net/ethernet/stmicro/stmmac/descs.h ++++ b/drivers/net/ethernet/stmicro/stmmac/descs.h +@@ -87,7 +87,7 @@ + #define TDES0_ERROR_SUMMARY BIT(15) + #define TDES0_IP_HEADER_ERROR BIT(16) + #define TDES0_TIME_STAMP_STATUS BIT(17) +-#define TDES0_OWN BIT(31) ++#define TDES0_OWN ((u32)BIT(31)) /* silence sparse */ + /* TDES1 */ + #define TDES1_BUFFER1_SIZE_MASK GENMASK(10, 0) + #define TDES1_BUFFER2_SIZE_MASK GENMASK(21, 11) +@@ -130,7 +130,7 @@ + #define ETDES0_FIRST_SEGMENT BIT(28) + #define ETDES0_LAST_SEGMENT BIT(29) + #define ETDES0_INTERRUPT BIT(30) +-#define ETDES0_OWN BIT(31) ++#define ETDES0_OWN ((u32)BIT(31)) /* silence sparse */ + /* TDES1 */ + #define ETDES1_BUFFER1_SIZE_MASK GENMASK(12, 0) + #define ETDES1_BUFFER2_SIZE_MASK GENMASK(28, 16) +@@ -170,19 +170,19 @@ + + /* Basic descriptor structure for normal and alternate descriptors */ + struct dma_desc { +- unsigned int des0; +- unsigned int des1; +- unsigned int des2; +- unsigned int des3; ++ __le32 des0; ++ __le32 des1; ++ __le32 des2; ++ __le32 des3; + }; + + /* Extended descriptor structure (e.g. >= databook 3.50a) */ + struct dma_extended_desc { + struct dma_desc basic; /* Basic descriptors */ +- unsigned int des4; /* Extended Status */ +- unsigned int des5; /* Reserved */ +- unsigned int des6; /* Tx/Rx Timestamp Low */ +- unsigned int des7; /* Tx/Rx Timestamp High */ ++ __le32 des4; /* Extended Status */ ++ __le32 des5; /* Reserved */ ++ __le32 des6; /* Tx/Rx Timestamp Low */ ++ __le32 des7; /* Tx/Rx Timestamp High */ + }; + + /* Transmit checksum insertion control */ +--- a/drivers/net/ethernet/stmicro/stmmac/descs_com.h ++++ b/drivers/net/ethernet/stmicro/stmmac/descs_com.h +@@ -35,47 +35,50 @@ + /* Enhanced descriptors */ + static inline void ehn_desc_rx_set_on_ring(struct dma_desc *p, int end) + { +- p->des1 |= ((BUF_SIZE_8KiB - 1) << ERDES1_BUFFER2_SIZE_SHIFT) +- & ERDES1_BUFFER2_SIZE_MASK; ++ p->des1 |= cpu_to_le32(((BUF_SIZE_8KiB - 1) ++ << ERDES1_BUFFER2_SIZE_SHIFT) ++ & ERDES1_BUFFER2_SIZE_MASK); + + if (end) +- p->des1 |= ERDES1_END_RING; ++ p->des1 |= cpu_to_le32(ERDES1_END_RING); + } + + static inline void enh_desc_end_tx_desc_on_ring(struct dma_desc *p, int end) + { + if (end) +- p->des0 |= ETDES0_END_RING; ++ p->des0 |= cpu_to_le32(ETDES0_END_RING); + else +- p->des0 &= ~ETDES0_END_RING; ++ p->des0 &= cpu_to_le32(~ETDES0_END_RING); + } + + static inline void enh_set_tx_desc_len_on_ring(struct dma_desc *p, int len) + { + if (unlikely(len > BUF_SIZE_4KiB)) { +- p->des1 |= (((len - BUF_SIZE_4KiB) << ETDES1_BUFFER2_SIZE_SHIFT) ++ p->des1 |= cpu_to_le32((((len - BUF_SIZE_4KiB) ++ << ETDES1_BUFFER2_SIZE_SHIFT) + & ETDES1_BUFFER2_SIZE_MASK) | (BUF_SIZE_4KiB +- & ETDES1_BUFFER1_SIZE_MASK); ++ & ETDES1_BUFFER1_SIZE_MASK)); + } else +- p->des1 |= (len & ETDES1_BUFFER1_SIZE_MASK); ++ p->des1 |= cpu_to_le32((len & ETDES1_BUFFER1_SIZE_MASK)); + } + + /* Normal descriptors */ + static inline void ndesc_rx_set_on_ring(struct dma_desc *p, int end) + { +- p->des1 |= ((BUF_SIZE_2KiB - 1) << RDES1_BUFFER2_SIZE_SHIFT) +- & RDES1_BUFFER2_SIZE_MASK; ++ p->des1 |= cpu_to_le32(((BUF_SIZE_2KiB - 1) ++ << RDES1_BUFFER2_SIZE_SHIFT) ++ & RDES1_BUFFER2_SIZE_MASK); + + if (end) +- p->des1 |= RDES1_END_RING; ++ p->des1 |= cpu_to_le32(RDES1_END_RING); + } + + static inline void ndesc_end_tx_desc_on_ring(struct dma_desc *p, int end) + { + if (end) +- p->des1 |= TDES1_END_RING; ++ p->des1 |= cpu_to_le32(TDES1_END_RING); + else +- p->des1 &= ~TDES1_END_RING; ++ p->des1 &= cpu_to_le32(~TDES1_END_RING); + } + + static inline void norm_set_tx_desc_len_on_ring(struct dma_desc *p, int len) +@@ -83,10 +86,11 @@ static inline void norm_set_tx_desc_len_ + if (unlikely(len > BUF_SIZE_2KiB)) { + unsigned int buffer1 = (BUF_SIZE_2KiB - 1) + & TDES1_BUFFER1_SIZE_MASK; +- p->des1 |= ((((len - buffer1) << TDES1_BUFFER2_SIZE_SHIFT) +- & TDES1_BUFFER2_SIZE_MASK) | buffer1); ++ p->des1 |= cpu_to_le32((((len - buffer1) ++ << TDES1_BUFFER2_SIZE_SHIFT) ++ & TDES1_BUFFER2_SIZE_MASK) | buffer1); + } else +- p->des1 |= (len & TDES1_BUFFER1_SIZE_MASK); ++ p->des1 |= cpu_to_le32((len & TDES1_BUFFER1_SIZE_MASK)); + } + + /* Specific functions used for Chain mode */ +@@ -94,32 +98,32 @@ static inline void norm_set_tx_desc_len_ + /* Enhanced descriptors */ + static inline void ehn_desc_rx_set_on_chain(struct dma_desc *p) + { +- p->des1 |= ERDES1_SECOND_ADDRESS_CHAINED; ++ p->des1 |= cpu_to_le32(ERDES1_SECOND_ADDRESS_CHAINED); + } + + static inline void enh_desc_end_tx_desc_on_chain(struct dma_desc *p) + { +- p->des0 |= ETDES0_SECOND_ADDRESS_CHAINED; ++ p->des0 |= cpu_to_le32(ETDES0_SECOND_ADDRESS_CHAINED); + } + + static inline void enh_set_tx_desc_len_on_chain(struct dma_desc *p, int len) + { +- p->des1 |= (len & ETDES1_BUFFER1_SIZE_MASK); ++ p->des1 |= cpu_to_le32(len & ETDES1_BUFFER1_SIZE_MASK); + } + + /* Normal descriptors */ + static inline void ndesc_rx_set_on_chain(struct dma_desc *p, int end) + { +- p->des1 |= RDES1_SECOND_ADDRESS_CHAINED; ++ p->des1 |= cpu_to_le32(RDES1_SECOND_ADDRESS_CHAINED); + } + + static inline void ndesc_tx_set_on_chain(struct dma_desc *p) + { +- p->des1 |= TDES1_SECOND_ADDRESS_CHAINED; ++ p->des1 |= cpu_to_le32(TDES1_SECOND_ADDRESS_CHAINED); + } + + static inline void norm_set_tx_desc_len_on_chain(struct dma_desc *p, int len) + { +- p->des1 |= len & TDES1_BUFFER1_SIZE_MASK; ++ p->des1 |= cpu_to_le32(len & TDES1_BUFFER1_SIZE_MASK); + } + #endif /* __DESC_COM_H__ */ +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-generic.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-generic.c +@@ -71,9 +71,12 @@ err_remove_config_dt: + + static const struct of_device_id dwmac_generic_match[] = { + { .compatible = "st,spear600-gmac"}, ++ { .compatible = "snps,dwmac-3.50a"}, + { .compatible = "snps,dwmac-3.610"}, + { .compatible = "snps,dwmac-3.70a"}, + { .compatible = "snps,dwmac-3.710"}, ++ { .compatible = "snps,dwmac-4.00"}, ++ { .compatible = "snps,dwmac-4.10a"}, + { .compatible = "snps,dwmac"}, + { } + }; +--- /dev/null ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-oxnas.c +@@ -0,0 +1,194 @@ ++/* ++ * Oxford Semiconductor OXNAS DWMAC glue layer ++ * ++ * Copyright (C) 2016 Neil Armstrong ++ * Copyright (C) 2014 Daniel Golle ++ * Copyright (C) 2013 Ma Haijun ++ * Copyright (C) 2012 John Crispin ++ * ++ * This program is free software; you can redistribute it and/or modify ++ * it under the terms of the GNU General Public License version 2 as ++ * published by the Free Software Foundation. ++ * ++ * You should have received a copy of the GNU General Public License ++ * along with this program. If not, see . ++ */ ++ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#include "stmmac_platform.h" ++ ++/* System Control regmap offsets */ ++#define OXNAS_DWMAC_CTRL_REGOFFSET 0x78 ++#define OXNAS_DWMAC_DELAY_REGOFFSET 0x100 ++ ++/* Control Register */ ++#define DWMAC_CKEN_RX_IN 14 ++#define DWMAC_CKEN_RXN_OUT 13 ++#define DWMAC_CKEN_RX_OUT 12 ++#define DWMAC_CKEN_TX_IN 10 ++#define DWMAC_CKEN_TXN_OUT 9 ++#define DWMAC_CKEN_TX_OUT 8 ++#define DWMAC_RX_SOURCE 7 ++#define DWMAC_TX_SOURCE 6 ++#define DWMAC_LOW_TX_SOURCE 4 ++#define DWMAC_AUTO_TX_SOURCE 3 ++#define DWMAC_RGMII 2 ++#define DWMAC_SIMPLE_MUX 1 ++#define DWMAC_CKEN_GTX 0 ++ ++/* Delay register */ ++#define DWMAC_TX_VARDELAY_SHIFT 0 ++#define DWMAC_TXN_VARDELAY_SHIFT 8 ++#define DWMAC_RX_VARDELAY_SHIFT 16 ++#define DWMAC_RXN_VARDELAY_SHIFT 24 ++#define DWMAC_TX_VARDELAY(d) ((d) << DWMAC_TX_VARDELAY_SHIFT) ++#define DWMAC_TXN_VARDELAY(d) ((d) << DWMAC_TXN_VARDELAY_SHIFT) ++#define DWMAC_RX_VARDELAY(d) ((d) << DWMAC_RX_VARDELAY_SHIFT) ++#define DWMAC_RXN_VARDELAY(d) ((d) << DWMAC_RXN_VARDELAY_SHIFT) ++ ++struct oxnas_dwmac { ++ struct device *dev; ++ struct clk *clk; ++ struct regmap *regmap; ++}; ++ ++static int oxnas_dwmac_init(struct platform_device *pdev, void *priv) ++{ ++ struct oxnas_dwmac *dwmac = priv; ++ unsigned int value; ++ int ret; ++ ++ /* Reset HW here before changing the glue configuration */ ++ ret = device_reset(dwmac->dev); ++ if (ret) ++ return ret; ++ ++ ret = clk_prepare_enable(dwmac->clk); ++ if (ret) ++ return ret; ++ ++ ret = regmap_read(dwmac->regmap, OXNAS_DWMAC_CTRL_REGOFFSET, &value); ++ if (ret < 0) { ++ clk_disable_unprepare(dwmac->clk); ++ return ret; ++ } ++ ++ /* Enable GMII_GTXCLK to follow GMII_REFCLK, required for gigabit PHY */ ++ value |= BIT(DWMAC_CKEN_GTX) | ++ /* Use simple mux for 25/125 Mhz clock switching */ ++ BIT(DWMAC_SIMPLE_MUX) | ++ /* set auto switch tx clock source */ ++ BIT(DWMAC_AUTO_TX_SOURCE) | ++ /* enable tx & rx vardelay */ ++ BIT(DWMAC_CKEN_TX_OUT) | ++ BIT(DWMAC_CKEN_TXN_OUT) | ++ BIT(DWMAC_CKEN_TX_IN) | ++ BIT(DWMAC_CKEN_RX_OUT) | ++ BIT(DWMAC_CKEN_RXN_OUT) | ++ BIT(DWMAC_CKEN_RX_IN); ++ regmap_write(dwmac->regmap, OXNAS_DWMAC_CTRL_REGOFFSET, value); ++ ++ /* set tx & rx vardelay */ ++ value = DWMAC_TX_VARDELAY(4) | ++ DWMAC_TXN_VARDELAY(2) | ++ DWMAC_RX_VARDELAY(10) | ++ DWMAC_RXN_VARDELAY(8); ++ regmap_write(dwmac->regmap, OXNAS_DWMAC_DELAY_REGOFFSET, value); ++ ++ return 0; ++} ++ ++static void oxnas_dwmac_exit(struct platform_device *pdev, void *priv) ++{ ++ struct oxnas_dwmac *dwmac = priv; ++ ++ clk_disable_unprepare(dwmac->clk); ++} ++ ++static int oxnas_dwmac_probe(struct platform_device *pdev) ++{ ++ struct plat_stmmacenet_data *plat_dat; ++ struct stmmac_resources stmmac_res; ++ struct oxnas_dwmac *dwmac; ++ int ret; ++ ++ ret = stmmac_get_platform_resources(pdev, &stmmac_res); ++ if (ret) ++ return ret; ++ ++ plat_dat = stmmac_probe_config_dt(pdev, &stmmac_res.mac); ++ if (IS_ERR(plat_dat)) ++ return PTR_ERR(plat_dat); ++ ++ dwmac = devm_kzalloc(&pdev->dev, sizeof(*dwmac), GFP_KERNEL); ++ if (!dwmac) { ++ ret = -ENOMEM; ++ goto err_remove_config_dt; ++ } ++ ++ dwmac->dev = &pdev->dev; ++ plat_dat->bsp_priv = dwmac; ++ plat_dat->init = oxnas_dwmac_init; ++ plat_dat->exit = oxnas_dwmac_exit; ++ ++ dwmac->regmap = syscon_regmap_lookup_by_phandle(pdev->dev.of_node, ++ "oxsemi,sys-ctrl"); ++ if (IS_ERR(dwmac->regmap)) { ++ dev_err(&pdev->dev, "failed to have sysctrl regmap\n"); ++ ret = PTR_ERR(dwmac->regmap); ++ goto err_remove_config_dt; ++ } ++ ++ dwmac->clk = devm_clk_get(&pdev->dev, "gmac"); ++ if (IS_ERR(dwmac->clk)) { ++ ret = PTR_ERR(dwmac->clk); ++ goto err_remove_config_dt; ++ } ++ ++ ret = oxnas_dwmac_init(pdev, plat_dat->bsp_priv); ++ if (ret) ++ goto err_remove_config_dt; ++ ++ ret = stmmac_dvr_probe(&pdev->dev, plat_dat, &stmmac_res); ++ if (ret) ++ goto err_dwmac_exit; ++ ++ ++ return 0; ++ ++err_dwmac_exit: ++ oxnas_dwmac_exit(pdev, plat_dat->bsp_priv); ++err_remove_config_dt: ++ stmmac_remove_config_dt(pdev, plat_dat); ++ ++ return ret; ++} ++ ++static const struct of_device_id oxnas_dwmac_match[] = { ++ { .compatible = "oxsemi,ox820-dwmac" }, ++ { } ++}; ++MODULE_DEVICE_TABLE(of, oxnas_dwmac_match); ++ ++static struct platform_driver oxnas_dwmac_driver = { ++ .probe = oxnas_dwmac_probe, ++ .remove = stmmac_pltfr_remove, ++ .driver = { ++ .name = "oxnas-dwmac", ++ .pm = &stmmac_pltfr_pm_ops, ++ .of_match_table = oxnas_dwmac_match, ++ }, ++}; ++module_platform_driver(oxnas_dwmac_driver); ++ ++MODULE_AUTHOR("Neil Armstrong "); ++MODULE_DESCRIPTION("Oxford Semiconductor OXNAS DWMAC glue layer"); ++MODULE_LICENSE("GPL v2"); +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c +@@ -864,6 +864,10 @@ static int rk_gmac_powerup(struct rk_pri + int ret; + struct device *dev = &bsp_priv->pdev->dev; + ++ ret = gmac_clk_enable(bsp_priv, true); ++ if (ret) ++ return ret; ++ + /*rmii or rgmii*/ + if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RGMII) { + dev_info(dev, "init for RGMII\n"); +@@ -880,10 +884,6 @@ static int rk_gmac_powerup(struct rk_pri + if (ret) + return ret; + +- ret = gmac_clk_enable(bsp_priv, true); +- if (ret) +- return ret; +- + pm_runtime_enable(dev); + pm_runtime_get_sync(dev); + +@@ -901,44 +901,6 @@ static void rk_gmac_powerdown(struct rk_ + gmac_clk_enable(gmac, false); + } + +-static int rk_gmac_init(struct platform_device *pdev, void *priv) +-{ +- struct rk_priv_data *bsp_priv = priv; +- +- return rk_gmac_powerup(bsp_priv); +-} +- +-static void rk_gmac_exit(struct platform_device *pdev, void *priv) +-{ +- struct rk_priv_data *bsp_priv = priv; +- +- rk_gmac_powerdown(bsp_priv); +-} +- +-static void rk_gmac_suspend(struct platform_device *pdev, void *priv) +-{ +- struct rk_priv_data *bsp_priv = priv; +- +- /* Keep the PHY up if we use Wake-on-Lan. */ +- if (device_may_wakeup(&pdev->dev)) +- return; +- +- rk_gmac_powerdown(bsp_priv); +- bsp_priv->suspended = true; +-} +- +-static void rk_gmac_resume(struct platform_device *pdev, void *priv) +-{ +- struct rk_priv_data *bsp_priv = priv; +- +- /* The PHY was up for Wake-on-Lan. */ +- if (!bsp_priv->suspended) +- return; +- +- rk_gmac_powerup(bsp_priv); +- bsp_priv->suspended = false; +-} +- + static void rk_fix_speed(void *priv, unsigned int speed) + { + struct rk_priv_data *bsp_priv = priv; +@@ -974,11 +936,7 @@ static int rk_gmac_probe(struct platform + return PTR_ERR(plat_dat); + + plat_dat->has_gmac = true; +- plat_dat->init = rk_gmac_init; +- plat_dat->exit = rk_gmac_exit; + plat_dat->fix_mac_speed = rk_fix_speed; +- plat_dat->suspend = rk_gmac_suspend; +- plat_dat->resume = rk_gmac_resume; + + plat_dat->bsp_priv = rk_gmac_setup(pdev, data); + if (IS_ERR(plat_dat->bsp_priv)) { +@@ -986,24 +944,65 @@ static int rk_gmac_probe(struct platform + goto err_remove_config_dt; + } + +- ret = rk_gmac_init(pdev, plat_dat->bsp_priv); ++ ret = rk_gmac_powerup(plat_dat->bsp_priv); + if (ret) + goto err_remove_config_dt; + + ret = stmmac_dvr_probe(&pdev->dev, plat_dat, &stmmac_res); + if (ret) +- goto err_gmac_exit; ++ goto err_gmac_powerdown; + + return 0; + +-err_gmac_exit: +- rk_gmac_exit(pdev, plat_dat->bsp_priv); ++err_gmac_powerdown: ++ rk_gmac_powerdown(plat_dat->bsp_priv); + err_remove_config_dt: + stmmac_remove_config_dt(pdev, plat_dat); + + return ret; + } + ++static int rk_gmac_remove(struct platform_device *pdev) ++{ ++ struct rk_priv_data *bsp_priv = get_stmmac_bsp_priv(&pdev->dev); ++ int ret = stmmac_dvr_remove(&pdev->dev); ++ ++ rk_gmac_powerdown(bsp_priv); ++ ++ return ret; ++} ++ ++#ifdef CONFIG_PM_SLEEP ++static int rk_gmac_suspend(struct device *dev) ++{ ++ struct rk_priv_data *bsp_priv = get_stmmac_bsp_priv(dev); ++ int ret = stmmac_suspend(dev); ++ ++ /* Keep the PHY up if we use Wake-on-Lan. */ ++ if (!device_may_wakeup(dev)) { ++ rk_gmac_powerdown(bsp_priv); ++ bsp_priv->suspended = true; ++ } ++ ++ return ret; ++} ++ ++static int rk_gmac_resume(struct device *dev) ++{ ++ struct rk_priv_data *bsp_priv = get_stmmac_bsp_priv(dev); ++ ++ /* The PHY was up for Wake-on-Lan. */ ++ if (bsp_priv->suspended) { ++ rk_gmac_powerup(bsp_priv); ++ bsp_priv->suspended = false; ++ } ++ ++ return stmmac_resume(dev); ++} ++#endif /* CONFIG_PM_SLEEP */ ++ ++static SIMPLE_DEV_PM_OPS(rk_gmac_pm_ops, rk_gmac_suspend, rk_gmac_resume); ++ + static const struct of_device_id rk_gmac_dwmac_match[] = { + { .compatible = "rockchip,rk3228-gmac", .data = &rk3228_ops }, + { .compatible = "rockchip,rk3288-gmac", .data = &rk3288_ops }, +@@ -1016,10 +1015,10 @@ MODULE_DEVICE_TABLE(of, rk_gmac_dwmac_ma + + static struct platform_driver rk_gmac_dwmac_driver = { + .probe = rk_gmac_probe, +- .remove = stmmac_pltfr_remove, ++ .remove = rk_gmac_remove, + .driver = { + .name = "rk_gmac-dwmac", +- .pm = &stmmac_pltfr_pm_ops, ++ .pm = &rk_gmac_pm_ops, + .of_match_table = rk_gmac_dwmac_match, + }, + }; +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c +@@ -380,8 +380,8 @@ static int socfpga_dwmac_resume(struct d + * control register 0, and can be modified by the phy driver + * framework. + */ +- if (priv->phydev) +- phy_resume(priv->phydev); ++ if (ndev->phydev) ++ phy_resume(ndev->phydev); + + return stmmac_resume(dev); + } +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c +@@ -126,8 +126,8 @@ struct sti_dwmac { + struct clk *clk; /* PHY clock */ + u32 ctrl_reg; /* GMAC glue-logic control register */ + int clk_sel_reg; /* GMAC ext clk selection register */ +- struct device *dev; + struct regmap *regmap; ++ bool gmac_en; + u32 speed; + void (*fix_retime_src)(void *priv, unsigned int speed); + }; +@@ -191,7 +191,7 @@ static void stih4xx_fix_retime_src(void + } + } + +- if (src == TX_RETIME_SRC_CLKGEN && dwmac->clk && freq) ++ if (src == TX_RETIME_SRC_CLKGEN && freq) + clk_set_rate(dwmac->clk, freq); + + regmap_update_bits(dwmac->regmap, reg, STIH4XX_RETIME_SRC_MASK, +@@ -222,26 +222,20 @@ static void stid127_fix_retime_src(void + freq = DWMAC_2_5MHZ; + } + +- if (dwmac->clk && freq) ++ if (freq) + clk_set_rate(dwmac->clk, freq); + + regmap_update_bits(dwmac->regmap, reg, STID127_RETIME_SRC_MASK, val); + } + +-static int sti_dwmac_init(struct platform_device *pdev, void *priv) ++static int sti_dwmac_set_mode(struct sti_dwmac *dwmac) + { +- struct sti_dwmac *dwmac = priv; + struct regmap *regmap = dwmac->regmap; + int iface = dwmac->interface; +- struct device *dev = dwmac->dev; +- struct device_node *np = dev->of_node; + u32 reg = dwmac->ctrl_reg; + u32 val; + +- if (dwmac->clk) +- clk_prepare_enable(dwmac->clk); +- +- if (of_property_read_bool(np, "st,gmac_en")) ++ if (dwmac->gmac_en) + regmap_update_bits(regmap, reg, EN_MASK, EN); + + regmap_update_bits(regmap, reg, MII_PHY_SEL_MASK, phy_intf_sels[iface]); +@@ -249,18 +243,11 @@ static int sti_dwmac_init(struct platfor + val = (iface == PHY_INTERFACE_MODE_REVMII) ? 0 : ENMII; + regmap_update_bits(regmap, reg, ENMII_MASK, val); + +- dwmac->fix_retime_src(priv, dwmac->speed); ++ dwmac->fix_retime_src(dwmac, dwmac->speed); + + return 0; + } + +-static void sti_dwmac_exit(struct platform_device *pdev, void *priv) +-{ +- struct sti_dwmac *dwmac = priv; +- +- if (dwmac->clk) +- clk_disable_unprepare(dwmac->clk); +-} + static int sti_dwmac_parse_data(struct sti_dwmac *dwmac, + struct platform_device *pdev) + { +@@ -270,9 +257,6 @@ static int sti_dwmac_parse_data(struct s + struct regmap *regmap; + int err; + +- if (!np) +- return -EINVAL; +- + /* clk selection from extra syscfg register */ + dwmac->clk_sel_reg = -ENXIO; + res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "sti-clkconf"); +@@ -289,9 +273,9 @@ static int sti_dwmac_parse_data(struct s + return err; + } + +- dwmac->dev = dev; + dwmac->interface = of_get_phy_mode(np); + dwmac->regmap = regmap; ++ dwmac->gmac_en = of_property_read_bool(np, "st,gmac_en"); + dwmac->ext_phyclk = of_property_read_bool(np, "st,ext-phyclk"); + dwmac->tx_retime_src = TX_RETIME_SRC_NA; + dwmac->speed = SPEED_100; +@@ -359,28 +343,65 @@ static int sti_dwmac_probe(struct platfo + dwmac->fix_retime_src = data->fix_retime_src; + + plat_dat->bsp_priv = dwmac; +- plat_dat->init = sti_dwmac_init; +- plat_dat->exit = sti_dwmac_exit; + plat_dat->fix_mac_speed = data->fix_retime_src; + +- ret = sti_dwmac_init(pdev, plat_dat->bsp_priv); ++ ret = clk_prepare_enable(dwmac->clk); + if (ret) + goto err_remove_config_dt; + ++ ret = sti_dwmac_set_mode(dwmac); ++ if (ret) ++ goto disable_clk; ++ + ret = stmmac_dvr_probe(&pdev->dev, plat_dat, &stmmac_res); + if (ret) +- goto err_dwmac_exit; ++ goto disable_clk; + + return 0; + +-err_dwmac_exit: +- sti_dwmac_exit(pdev, plat_dat->bsp_priv); ++disable_clk: ++ clk_disable_unprepare(dwmac->clk); + err_remove_config_dt: + stmmac_remove_config_dt(pdev, plat_dat); + + return ret; + } + ++static int sti_dwmac_remove(struct platform_device *pdev) ++{ ++ struct sti_dwmac *dwmac = get_stmmac_bsp_priv(&pdev->dev); ++ int ret = stmmac_dvr_remove(&pdev->dev); ++ ++ clk_disable_unprepare(dwmac->clk); ++ ++ return ret; ++} ++ ++#ifdef CONFIG_PM_SLEEP ++static int sti_dwmac_suspend(struct device *dev) ++{ ++ struct sti_dwmac *dwmac = get_stmmac_bsp_priv(dev); ++ int ret = stmmac_suspend(dev); ++ ++ clk_disable_unprepare(dwmac->clk); ++ ++ return ret; ++} ++ ++static int sti_dwmac_resume(struct device *dev) ++{ ++ struct sti_dwmac *dwmac = get_stmmac_bsp_priv(dev); ++ ++ clk_prepare_enable(dwmac->clk); ++ sti_dwmac_set_mode(dwmac); ++ ++ return stmmac_resume(dev); ++} ++#endif /* CONFIG_PM_SLEEP */ ++ ++static SIMPLE_DEV_PM_OPS(sti_dwmac_pm_ops, sti_dwmac_suspend, ++ sti_dwmac_resume); ++ + static const struct sti_dwmac_of_data stih4xx_dwmac_data = { + .fix_retime_src = stih4xx_fix_retime_src, + }; +@@ -400,10 +421,10 @@ MODULE_DEVICE_TABLE(of, sti_dwmac_match) + + static struct platform_driver sti_dwmac_driver = { + .probe = sti_dwmac_probe, +- .remove = stmmac_pltfr_remove, ++ .remove = sti_dwmac_remove, + .driver = { + .name = "sti-dwmac", +- .pm = &stmmac_pltfr_pm_ops, ++ .pm = &sti_dwmac_pm_ops, + .of_match_table = sti_dwmac_match, + }, + }; +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h +@@ -225,7 +225,7 @@ enum rx_tx_priority_ratio { + + #define DMA_BUS_MODE_FB 0x00010000 /* Fixed burst */ + #define DMA_BUS_MODE_MB 0x04000000 /* Mixed burst */ +-#define DMA_BUS_MODE_RPBL_MASK 0x003e0000 /* Rx-Programmable Burst Len */ ++#define DMA_BUS_MODE_RPBL_MASK 0x007e0000 /* Rx-Programmable Burst Len */ + #define DMA_BUS_MODE_RPBL_SHIFT 17 + #define DMA_BUS_MODE_USP 0x00800000 + #define DMA_BUS_MODE_MAXPBL 0x01000000 +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c +@@ -538,6 +538,12 @@ struct mac_device_info *dwmac1000_setup( + mac->link.speed = GMAC_CONTROL_FES; + mac->mii.addr = GMAC_MII_ADDR; + mac->mii.data = GMAC_MII_DATA; ++ mac->mii.addr_shift = 11; ++ mac->mii.addr_mask = 0x0000F800; ++ mac->mii.reg_shift = 6; ++ mac->mii.reg_mask = 0x000007C0; ++ mac->mii.clk_csr_shift = 2; ++ mac->mii.clk_csr_mask = GENMASK(5, 2); + + /* Get and dump the chip ID */ + *synopsys_id = stmmac_get_synopsys_id(hwid); +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c +@@ -84,37 +84,39 @@ static void dwmac1000_dma_axi(void __iom + writel(value, ioaddr + DMA_AXI_BUS_MODE); + } + +-static void dwmac1000_dma_init(void __iomem *ioaddr, int pbl, int fb, int mb, +- int aal, u32 dma_tx, u32 dma_rx, int atds) ++static void dwmac1000_dma_init(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, ++ u32 dma_tx, u32 dma_rx, int atds) + { + u32 value = readl(ioaddr + DMA_BUS_MODE); ++ int txpbl = dma_cfg->txpbl ?: dma_cfg->pbl; ++ int rxpbl = dma_cfg->rxpbl ?: dma_cfg->pbl; + + /* + * Set the DMA PBL (Programmable Burst Length) mode. + * + * Note: before stmmac core 3.50 this mode bit was 4xPBL, and + * post 3.5 mode bit acts as 8*PBL. +- * +- * This configuration doesn't take care about the Separate PBL +- * so only the bits: 13-8 are programmed with the PBL passed from the +- * platform. + */ +- value |= DMA_BUS_MODE_MAXPBL; +- value &= ~DMA_BUS_MODE_PBL_MASK; +- value |= (pbl << DMA_BUS_MODE_PBL_SHIFT); ++ if (dma_cfg->pblx8) ++ value |= DMA_BUS_MODE_MAXPBL; ++ value |= DMA_BUS_MODE_USP; ++ value &= ~(DMA_BUS_MODE_PBL_MASK | DMA_BUS_MODE_RPBL_MASK); ++ value |= (txpbl << DMA_BUS_MODE_PBL_SHIFT); ++ value |= (rxpbl << DMA_BUS_MODE_RPBL_SHIFT); + + /* Set the Fixed burst mode */ +- if (fb) ++ if (dma_cfg->fixed_burst) + value |= DMA_BUS_MODE_FB; + + /* Mixed Burst has no effect when fb is set */ +- if (mb) ++ if (dma_cfg->mixed_burst) + value |= DMA_BUS_MODE_MB; + + if (atds) + value |= DMA_BUS_MODE_ATDS; + +- if (aal) ++ if (dma_cfg->aal) + value |= DMA_BUS_MODE_AAL; + + writel(value, ioaddr + DMA_BUS_MODE); +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c +@@ -192,6 +192,13 @@ struct mac_device_info *dwmac100_setup(v + mac->link.speed = 0; + mac->mii.addr = MAC_MII_ADDR; + mac->mii.data = MAC_MII_DATA; ++ mac->mii.addr_shift = 11; ++ mac->mii.addr_mask = 0x0000F800; ++ mac->mii.reg_shift = 6; ++ mac->mii.reg_mask = 0x000007C0; ++ mac->mii.clk_csr_shift = 2; ++ mac->mii.clk_csr_mask = GENMASK(5, 2); ++ + /* Synopsys Id is not available on old chips */ + *synopsys_id = 0; + +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100_dma.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100_dma.c +@@ -32,11 +32,12 @@ + #include "dwmac100.h" + #include "dwmac_dma.h" + +-static void dwmac100_dma_init(void __iomem *ioaddr, int pbl, int fb, int mb, +- int aal, u32 dma_tx, u32 dma_rx, int atds) ++static void dwmac100_dma_init(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, ++ u32 dma_tx, u32 dma_rx, int atds) + { + /* Enable Application Access by writing to DMA CSR0 */ +- writel(DMA_BUS_MODE_DEFAULT | (pbl << DMA_BUS_MODE_PBL_SHIFT), ++ writel(DMA_BUS_MODE_DEFAULT | (dma_cfg->pbl << DMA_BUS_MODE_PBL_SHIFT), + ioaddr + DMA_BUS_MODE); + + /* Mask interrupts by writing to CSR7 */ +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4.h ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4.h +@@ -155,8 +155,11 @@ enum power_event { + #define MTL_CHAN_RX_DEBUG(x) (MTL_CHANX_BASE_ADDR(x) + 0x38) + + #define MTL_OP_MODE_RSF BIT(5) ++#define MTL_OP_MODE_TXQEN BIT(3) + #define MTL_OP_MODE_TSF BIT(1) + ++#define MTL_OP_MODE_TQS_MASK GENMASK(24, 16) ++ + #define MTL_OP_MODE_TTC_MASK 0x70 + #define MTL_OP_MODE_TTC_SHIFT 4 + +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c +@@ -430,6 +430,12 @@ struct mac_device_info *dwmac4_setup(voi + mac->link.speed = GMAC_CONFIG_FES; + mac->mii.addr = GMAC_MDIO_ADDR; + mac->mii.data = GMAC_MDIO_DATA; ++ mac->mii.addr_shift = 21; ++ mac->mii.addr_mask = GENMASK(25, 21); ++ mac->mii.reg_shift = 16; ++ mac->mii.reg_mask = GENMASK(20, 16); ++ mac->mii.clk_csr_shift = 8; ++ mac->mii.clk_csr_mask = GENMASK(11, 8); + + /* Get and dump the chip ID */ + *synopsys_id = stmmac_get_synopsys_id(hwid); +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c +@@ -23,7 +23,7 @@ static int dwmac4_wrback_get_tx_status(v + unsigned int tdes3; + int ret = tx_done; + +- tdes3 = p->des3; ++ tdes3 = le32_to_cpu(p->des3); + + /* Get tx owner first */ + if (unlikely(tdes3 & TDES3_OWN)) +@@ -77,9 +77,9 @@ static int dwmac4_wrback_get_rx_status(v + struct dma_desc *p) + { + struct net_device_stats *stats = (struct net_device_stats *)data; +- unsigned int rdes1 = p->des1; +- unsigned int rdes2 = p->des2; +- unsigned int rdes3 = p->des3; ++ unsigned int rdes1 = le32_to_cpu(p->des1); ++ unsigned int rdes2 = le32_to_cpu(p->des2); ++ unsigned int rdes3 = le32_to_cpu(p->des3); + int message_type; + int ret = good_frame; + +@@ -176,47 +176,48 @@ static int dwmac4_wrback_get_rx_status(v + + static int dwmac4_rd_get_tx_len(struct dma_desc *p) + { +- return (p->des2 & TDES2_BUFFER1_SIZE_MASK); ++ return (le32_to_cpu(p->des2) & TDES2_BUFFER1_SIZE_MASK); + } + + static int dwmac4_get_tx_owner(struct dma_desc *p) + { +- return (p->des3 & TDES3_OWN) >> TDES3_OWN_SHIFT; ++ return (le32_to_cpu(p->des3) & TDES3_OWN) >> TDES3_OWN_SHIFT; + } + + static void dwmac4_set_tx_owner(struct dma_desc *p) + { +- p->des3 |= TDES3_OWN; ++ p->des3 |= cpu_to_le32(TDES3_OWN); + } + + static void dwmac4_set_rx_owner(struct dma_desc *p) + { +- p->des3 |= RDES3_OWN; ++ p->des3 |= cpu_to_le32(RDES3_OWN); + } + + static int dwmac4_get_tx_ls(struct dma_desc *p) + { +- return (p->des3 & TDES3_LAST_DESCRIPTOR) >> TDES3_LAST_DESCRIPTOR_SHIFT; ++ return (le32_to_cpu(p->des3) & TDES3_LAST_DESCRIPTOR) ++ >> TDES3_LAST_DESCRIPTOR_SHIFT; + } + + static int dwmac4_wrback_get_rx_frame_len(struct dma_desc *p, int rx_coe) + { +- return (p->des3 & RDES3_PACKET_SIZE_MASK); ++ return (le32_to_cpu(p->des3) & RDES3_PACKET_SIZE_MASK); + } + + static void dwmac4_rd_enable_tx_timestamp(struct dma_desc *p) + { +- p->des2 |= TDES2_TIMESTAMP_ENABLE; ++ p->des2 |= cpu_to_le32(TDES2_TIMESTAMP_ENABLE); + } + + static int dwmac4_wrback_get_tx_timestamp_status(struct dma_desc *p) + { + /* Context type from W/B descriptor must be zero */ +- if (p->des3 & TDES3_CONTEXT_TYPE) ++ if (le32_to_cpu(p->des3) & TDES3_CONTEXT_TYPE) + return -EINVAL; + + /* Tx Timestamp Status is 1 so des0 and des1'll have valid values */ +- if (p->des3 & TDES3_TIMESTAMP_STATUS) ++ if (le32_to_cpu(p->des3) & TDES3_TIMESTAMP_STATUS) + return 0; + + return 1; +@@ -227,9 +228,9 @@ static inline u64 dwmac4_get_timestamp(v + struct dma_desc *p = (struct dma_desc *)desc; + u64 ns; + +- ns = p->des0; ++ ns = le32_to_cpu(p->des0); + /* convert high/sec time stamp value to nanosecond */ +- ns += p->des1 * 1000000000ULL; ++ ns += le32_to_cpu(p->des1) * 1000000000ULL; + + return ns; + } +@@ -264,7 +265,7 @@ static int dwmac4_wrback_get_rx_timestam + + /* Get the status from normal w/b descriptor */ + if (likely(p->des3 & TDES3_RS1V)) { +- if (likely(p->des1 & RDES1_TIMESTAMP_AVAILABLE)) { ++ if (likely(le32_to_cpu(p->des1) & RDES1_TIMESTAMP_AVAILABLE)) { + int i = 0; + + /* Check if timestamp is OK from context descriptor */ +@@ -287,10 +288,10 @@ exit: + static void dwmac4_rd_init_rx_desc(struct dma_desc *p, int disable_rx_ic, + int mode, int end) + { +- p->des3 = RDES3_OWN | RDES3_BUFFER1_VALID_ADDR; ++ p->des3 = cpu_to_le32(RDES3_OWN | RDES3_BUFFER1_VALID_ADDR); + + if (!disable_rx_ic) +- p->des3 |= RDES3_INT_ON_COMPLETION_EN; ++ p->des3 |= cpu_to_le32(RDES3_INT_ON_COMPLETION_EN); + } + + static void dwmac4_rd_init_tx_desc(struct dma_desc *p, int mode, int end) +@@ -305,9 +306,9 @@ static void dwmac4_rd_prepare_tx_desc(st + bool csum_flag, int mode, bool tx_own, + bool ls) + { +- unsigned int tdes3 = p->des3; ++ unsigned int tdes3 = le32_to_cpu(p->des3); + +- p->des2 |= (len & TDES2_BUFFER1_SIZE_MASK); ++ p->des2 |= cpu_to_le32(len & TDES2_BUFFER1_SIZE_MASK); + + if (is_fs) + tdes3 |= TDES3_FIRST_DESCRIPTOR; +@@ -333,9 +334,9 @@ static void dwmac4_rd_prepare_tx_desc(st + * descriptors for the same frame has to be set before, to + * avoid race condition. + */ +- wmb(); ++ dma_wmb(); + +- p->des3 = tdes3; ++ p->des3 = cpu_to_le32(tdes3); + } + + static void dwmac4_rd_prepare_tso_tx_desc(struct dma_desc *p, int is_fs, +@@ -343,14 +344,14 @@ static void dwmac4_rd_prepare_tso_tx_des + bool ls, unsigned int tcphdrlen, + unsigned int tcppayloadlen) + { +- unsigned int tdes3 = p->des3; ++ unsigned int tdes3 = le32_to_cpu(p->des3); + + if (len1) +- p->des2 |= (len1 & TDES2_BUFFER1_SIZE_MASK); ++ p->des2 |= cpu_to_le32((len1 & TDES2_BUFFER1_SIZE_MASK)); + + if (len2) +- p->des2 |= (len2 << TDES2_BUFFER2_SIZE_MASK_SHIFT) +- & TDES2_BUFFER2_SIZE_MASK; ++ p->des2 |= cpu_to_le32((len2 << TDES2_BUFFER2_SIZE_MASK_SHIFT) ++ & TDES2_BUFFER2_SIZE_MASK); + + if (is_fs) { + tdes3 |= TDES3_FIRST_DESCRIPTOR | +@@ -376,9 +377,9 @@ static void dwmac4_rd_prepare_tso_tx_des + * descriptors for the same frame has to be set before, to + * avoid race condition. + */ +- wmb(); ++ dma_wmb(); + +- p->des3 = tdes3; ++ p->des3 = cpu_to_le32(tdes3); + } + + static void dwmac4_release_tx_desc(struct dma_desc *p, int mode) +@@ -389,7 +390,7 @@ static void dwmac4_release_tx_desc(struc + + static void dwmac4_rd_set_tx_ic(struct dma_desc *p) + { +- p->des2 |= TDES2_INTERRUPT_ON_COMPLETION; ++ p->des2 |= cpu_to_le32(TDES2_INTERRUPT_ON_COMPLETION); + } + + static void dwmac4_display_ring(void *head, unsigned int size, bool rx) +@@ -402,7 +403,8 @@ static void dwmac4_display_ring(void *he + for (i = 0; i < size; i++) { + pr_info("%d [0x%x]: 0x%x 0x%x 0x%x 0x%x\n", + i, (unsigned int)virt_to_phys(p), +- p->des0, p->des1, p->des2, p->des3); ++ le32_to_cpu(p->des0), le32_to_cpu(p->des1), ++ le32_to_cpu(p->des2), le32_to_cpu(p->des3)); + p++; + } + } +@@ -411,8 +413,8 @@ static void dwmac4_set_mss_ctxt(struct d + { + p->des0 = 0; + p->des1 = 0; +- p->des2 = mss; +- p->des3 = TDES3_CONTEXT_TYPE | TDES3_CTXT_TCMSSV; ++ p->des2 = cpu_to_le32(mss); ++ p->des3 = cpu_to_le32(TDES3_CONTEXT_TYPE | TDES3_CTXT_TCMSSV); + } + + const struct stmmac_desc_ops dwmac4_desc_ops = { +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c +@@ -71,25 +71,29 @@ static void dwmac4_dma_axi(void __iomem + writel(value, ioaddr + DMA_SYS_BUS_MODE); + } + +-static void dwmac4_dma_init_channel(void __iomem *ioaddr, int pbl, ++static void dwmac4_dma_init_channel(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, + u32 dma_tx_phy, u32 dma_rx_phy, + u32 channel) + { + u32 value; ++ int txpbl = dma_cfg->txpbl ?: dma_cfg->pbl; ++ int rxpbl = dma_cfg->rxpbl ?: dma_cfg->pbl; + + /* set PBL for each channels. Currently we affect same configuration + * on each channel + */ + value = readl(ioaddr + DMA_CHAN_CONTROL(channel)); +- value = value | DMA_BUS_MODE_PBL; ++ if (dma_cfg->pblx8) ++ value = value | DMA_BUS_MODE_PBL; + writel(value, ioaddr + DMA_CHAN_CONTROL(channel)); + + value = readl(ioaddr + DMA_CHAN_TX_CONTROL(channel)); +- value = value | (pbl << DMA_BUS_MODE_PBL_SHIFT); ++ value = value | (txpbl << DMA_BUS_MODE_PBL_SHIFT); + writel(value, ioaddr + DMA_CHAN_TX_CONTROL(channel)); + + value = readl(ioaddr + DMA_CHAN_RX_CONTROL(channel)); +- value = value | (pbl << DMA_BUS_MODE_RPBL_SHIFT); ++ value = value | (rxpbl << DMA_BUS_MODE_RPBL_SHIFT); + writel(value, ioaddr + DMA_CHAN_RX_CONTROL(channel)); + + /* Mask interrupts by writing to CSR7 */ +@@ -99,27 +103,28 @@ static void dwmac4_dma_init_channel(void + writel(dma_rx_phy, ioaddr + DMA_CHAN_RX_BASE_ADDR(channel)); + } + +-static void dwmac4_dma_init(void __iomem *ioaddr, int pbl, int fb, int mb, +- int aal, u32 dma_tx, u32 dma_rx, int atds) ++static void dwmac4_dma_init(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, ++ u32 dma_tx, u32 dma_rx, int atds) + { + u32 value = readl(ioaddr + DMA_SYS_BUS_MODE); + int i; + + /* Set the Fixed burst mode */ +- if (fb) ++ if (dma_cfg->fixed_burst) + value |= DMA_SYS_BUS_FB; + + /* Mixed Burst has no effect when fb is set */ +- if (mb) ++ if (dma_cfg->mixed_burst) + value |= DMA_SYS_BUS_MB; + +- if (aal) ++ if (dma_cfg->aal) + value |= DMA_SYS_BUS_AAL; + + writel(value, ioaddr + DMA_SYS_BUS_MODE); + + for (i = 0; i < DMA_CHANNEL_NB_MAX; i++) +- dwmac4_dma_init_channel(ioaddr, pbl, dma_tx, dma_rx, i); ++ dwmac4_dma_init_channel(ioaddr, dma_cfg, dma_tx, dma_rx, i); + } + + static void _dwmac4_dump_dma_regs(void __iomem *ioaddr, u32 channel) +@@ -215,7 +220,17 @@ static void dwmac4_dma_chan_op_mode(void + else + mtl_tx_op |= MTL_OP_MODE_TTC_512; + } +- ++ /* For an IP with DWC_EQOS_NUM_TXQ == 1, the fields TXQEN and TQS are RO ++ * with reset values: TXQEN on, TQS == DWC_EQOS_TXFIFO_SIZE. ++ * For an IP with DWC_EQOS_NUM_TXQ > 1, the fields TXQEN and TQS are R/W ++ * with reset values: TXQEN off, TQS 256 bytes. ++ * ++ * Write the bits in both cases, since it will have no effect when RO. ++ * For DWC_EQOS_NUM_TXQ > 1, the top bits in MTL_OP_MODE_TQS_MASK might ++ * be RO, however, writing the whole TQS field will result in a value ++ * equal to DWC_EQOS_TXFIFO_SIZE, just like for DWC_EQOS_NUM_TXQ == 1. ++ */ ++ mtl_tx_op |= MTL_OP_MODE_TXQEN | MTL_OP_MODE_TQS_MASK; + writel(mtl_tx_op, ioaddr + MTL_CHAN_TX_OP_MODE(channel)); + + mtl_rx_op = readl(ioaddr + MTL_CHAN_RX_OP_MODE(channel)); +--- a/drivers/net/ethernet/stmicro/stmmac/enh_desc.c ++++ b/drivers/net/ethernet/stmicro/stmmac/enh_desc.c +@@ -30,7 +30,7 @@ static int enh_desc_get_tx_status(void * + struct dma_desc *p, void __iomem *ioaddr) + { + struct net_device_stats *stats = (struct net_device_stats *)data; +- unsigned int tdes0 = p->des0; ++ unsigned int tdes0 = le32_to_cpu(p->des0); + int ret = tx_done; + + /* Get tx owner first */ +@@ -95,7 +95,7 @@ static int enh_desc_get_tx_status(void * + + static int enh_desc_get_tx_len(struct dma_desc *p) + { +- return (p->des1 & ETDES1_BUFFER1_SIZE_MASK); ++ return (le32_to_cpu(p->des1) & ETDES1_BUFFER1_SIZE_MASK); + } + + static int enh_desc_coe_rdes0(int ipc_err, int type, int payload_err) +@@ -134,8 +134,8 @@ static int enh_desc_coe_rdes0(int ipc_er + static void enh_desc_get_ext_status(void *data, struct stmmac_extra_stats *x, + struct dma_extended_desc *p) + { +- unsigned int rdes0 = p->basic.des0; +- unsigned int rdes4 = p->des4; ++ unsigned int rdes0 = le32_to_cpu(p->basic.des0); ++ unsigned int rdes4 = le32_to_cpu(p->des4); + + if (unlikely(rdes0 & ERDES0_RX_MAC_ADDR)) { + int message_type = (rdes4 & ERDES4_MSG_TYPE_MASK) >> 8; +@@ -199,7 +199,7 @@ static int enh_desc_get_rx_status(void * + struct dma_desc *p) + { + struct net_device_stats *stats = (struct net_device_stats *)data; +- unsigned int rdes0 = p->des0; ++ unsigned int rdes0 = le32_to_cpu(p->des0); + int ret = good_frame; + + if (unlikely(rdes0 & RDES0_OWN)) +@@ -265,8 +265,8 @@ static int enh_desc_get_rx_status(void * + static void enh_desc_init_rx_desc(struct dma_desc *p, int disable_rx_ic, + int mode, int end) + { +- p->des0 |= RDES0_OWN; +- p->des1 |= ((BUF_SIZE_8KiB - 1) & ERDES1_BUFFER1_SIZE_MASK); ++ p->des0 |= cpu_to_le32(RDES0_OWN); ++ p->des1 |= cpu_to_le32((BUF_SIZE_8KiB - 1) & ERDES1_BUFFER1_SIZE_MASK); + + if (mode == STMMAC_CHAIN_MODE) + ehn_desc_rx_set_on_chain(p); +@@ -274,12 +274,12 @@ static void enh_desc_init_rx_desc(struct + ehn_desc_rx_set_on_ring(p, end); + + if (disable_rx_ic) +- p->des1 |= ERDES1_DISABLE_IC; ++ p->des1 |= cpu_to_le32(ERDES1_DISABLE_IC); + } + + static void enh_desc_init_tx_desc(struct dma_desc *p, int mode, int end) + { +- p->des0 &= ~ETDES0_OWN; ++ p->des0 &= cpu_to_le32(~ETDES0_OWN); + if (mode == STMMAC_CHAIN_MODE) + enh_desc_end_tx_desc_on_chain(p); + else +@@ -288,27 +288,27 @@ static void enh_desc_init_tx_desc(struct + + static int enh_desc_get_tx_owner(struct dma_desc *p) + { +- return (p->des0 & ETDES0_OWN) >> 31; ++ return (le32_to_cpu(p->des0) & ETDES0_OWN) >> 31; + } + + static void enh_desc_set_tx_owner(struct dma_desc *p) + { +- p->des0 |= ETDES0_OWN; ++ p->des0 |= cpu_to_le32(ETDES0_OWN); + } + + static void enh_desc_set_rx_owner(struct dma_desc *p) + { +- p->des0 |= RDES0_OWN; ++ p->des0 |= cpu_to_le32(RDES0_OWN); + } + + static int enh_desc_get_tx_ls(struct dma_desc *p) + { +- return (p->des0 & ETDES0_LAST_SEGMENT) >> 29; ++ return (le32_to_cpu(p->des0) & ETDES0_LAST_SEGMENT) >> 29; + } + + static void enh_desc_release_tx_desc(struct dma_desc *p, int mode) + { +- int ter = (p->des0 & ETDES0_END_RING) >> 21; ++ int ter = (le32_to_cpu(p->des0) & ETDES0_END_RING) >> 21; + + memset(p, 0, offsetof(struct dma_desc, des2)); + if (mode == STMMAC_CHAIN_MODE) +@@ -321,7 +321,7 @@ static void enh_desc_prepare_tx_desc(str + bool csum_flag, int mode, bool tx_own, + bool ls) + { +- unsigned int tdes0 = p->des0; ++ unsigned int tdes0 = le32_to_cpu(p->des0); + + if (mode == STMMAC_CHAIN_MODE) + enh_set_tx_desc_len_on_chain(p, len); +@@ -350,14 +350,14 @@ static void enh_desc_prepare_tx_desc(str + * descriptors for the same frame has to be set before, to + * avoid race condition. + */ +- wmb(); ++ dma_wmb(); + +- p->des0 = tdes0; ++ p->des0 = cpu_to_le32(tdes0); + } + + static void enh_desc_set_tx_ic(struct dma_desc *p) + { +- p->des0 |= ETDES0_INTERRUPT; ++ p->des0 |= cpu_to_le32(ETDES0_INTERRUPT); + } + + static int enh_desc_get_rx_frame_len(struct dma_desc *p, int rx_coe_type) +@@ -372,18 +372,18 @@ static int enh_desc_get_rx_frame_len(str + if (rx_coe_type == STMMAC_RX_COE_TYPE1) + csum = 2; + +- return (((p->des0 & RDES0_FRAME_LEN_MASK) >> RDES0_FRAME_LEN_SHIFT) - +- csum); ++ return (((le32_to_cpu(p->des0) & RDES0_FRAME_LEN_MASK) ++ >> RDES0_FRAME_LEN_SHIFT) - csum); + } + + static void enh_desc_enable_tx_timestamp(struct dma_desc *p) + { +- p->des0 |= ETDES0_TIME_STAMP_ENABLE; ++ p->des0 |= cpu_to_le32(ETDES0_TIME_STAMP_ENABLE); + } + + static int enh_desc_get_tx_timestamp_status(struct dma_desc *p) + { +- return (p->des0 & ETDES0_TIME_STAMP_STATUS) >> 17; ++ return (le32_to_cpu(p->des0) & ETDES0_TIME_STAMP_STATUS) >> 17; + } + + static u64 enh_desc_get_timestamp(void *desc, u32 ats) +@@ -392,13 +392,13 @@ static u64 enh_desc_get_timestamp(void * + + if (ats) { + struct dma_extended_desc *p = (struct dma_extended_desc *)desc; +- ns = p->des6; ++ ns = le32_to_cpu(p->des6); + /* convert high/sec time stamp value to nanosecond */ +- ns += p->des7 * 1000000000ULL; ++ ns += le32_to_cpu(p->des7) * 1000000000ULL; + } else { + struct dma_desc *p = (struct dma_desc *)desc; +- ns = p->des2; +- ns += p->des3 * 1000000000ULL; ++ ns = le32_to_cpu(p->des2); ++ ns += le32_to_cpu(p->des3) * 1000000000ULL; + } + + return ns; +@@ -408,10 +408,11 @@ static int enh_desc_get_rx_timestamp_sta + { + if (ats) { + struct dma_extended_desc *p = (struct dma_extended_desc *)desc; +- return (p->basic.des0 & RDES0_IPC_CSUM_ERROR) >> 7; ++ return (le32_to_cpu(p->basic.des0) & RDES0_IPC_CSUM_ERROR) >> 7; + } else { + struct dma_desc *p = (struct dma_desc *)desc; +- if ((p->des2 == 0xffffffff) && (p->des3 == 0xffffffff)) ++ if ((le32_to_cpu(p->des2) == 0xffffffff) && ++ (le32_to_cpu(p->des3) == 0xffffffff)) + /* timestamp is corrupted, hence don't store it */ + return 0; + else +--- a/drivers/net/ethernet/stmicro/stmmac/norm_desc.c ++++ b/drivers/net/ethernet/stmicro/stmmac/norm_desc.c +@@ -30,8 +30,8 @@ static int ndesc_get_tx_status(void *dat + struct dma_desc *p, void __iomem *ioaddr) + { + struct net_device_stats *stats = (struct net_device_stats *)data; +- unsigned int tdes0 = p->des0; +- unsigned int tdes1 = p->des1; ++ unsigned int tdes0 = le32_to_cpu(p->des0); ++ unsigned int tdes1 = le32_to_cpu(p->des1); + int ret = tx_done; + + /* Get tx owner first */ +@@ -77,7 +77,7 @@ static int ndesc_get_tx_status(void *dat + + static int ndesc_get_tx_len(struct dma_desc *p) + { +- return (p->des1 & RDES1_BUFFER1_SIZE_MASK); ++ return (le32_to_cpu(p->des1) & RDES1_BUFFER1_SIZE_MASK); + } + + /* This function verifies if each incoming frame has some errors +@@ -88,7 +88,7 @@ static int ndesc_get_rx_status(void *dat + struct dma_desc *p) + { + int ret = good_frame; +- unsigned int rdes0 = p->des0; ++ unsigned int rdes0 = le32_to_cpu(p->des0); + struct net_device_stats *stats = (struct net_device_stats *)data; + + if (unlikely(rdes0 & RDES0_OWN)) +@@ -141,8 +141,8 @@ static int ndesc_get_rx_status(void *dat + static void ndesc_init_rx_desc(struct dma_desc *p, int disable_rx_ic, int mode, + int end) + { +- p->des0 |= RDES0_OWN; +- p->des1 |= (BUF_SIZE_2KiB - 1) & RDES1_BUFFER1_SIZE_MASK; ++ p->des0 |= cpu_to_le32(RDES0_OWN); ++ p->des1 |= cpu_to_le32((BUF_SIZE_2KiB - 1) & RDES1_BUFFER1_SIZE_MASK); + + if (mode == STMMAC_CHAIN_MODE) + ndesc_rx_set_on_chain(p, end); +@@ -150,12 +150,12 @@ static void ndesc_init_rx_desc(struct dm + ndesc_rx_set_on_ring(p, end); + + if (disable_rx_ic) +- p->des1 |= RDES1_DISABLE_IC; ++ p->des1 |= cpu_to_le32(RDES1_DISABLE_IC); + } + + static void ndesc_init_tx_desc(struct dma_desc *p, int mode, int end) + { +- p->des0 &= ~TDES0_OWN; ++ p->des0 &= cpu_to_le32(~TDES0_OWN); + if (mode == STMMAC_CHAIN_MODE) + ndesc_tx_set_on_chain(p); + else +@@ -164,27 +164,27 @@ static void ndesc_init_tx_desc(struct dm + + static int ndesc_get_tx_owner(struct dma_desc *p) + { +- return (p->des0 & TDES0_OWN) >> 31; ++ return (le32_to_cpu(p->des0) & TDES0_OWN) >> 31; + } + + static void ndesc_set_tx_owner(struct dma_desc *p) + { +- p->des0 |= TDES0_OWN; ++ p->des0 |= cpu_to_le32(TDES0_OWN); + } + + static void ndesc_set_rx_owner(struct dma_desc *p) + { +- p->des0 |= RDES0_OWN; ++ p->des0 |= cpu_to_le32(RDES0_OWN); + } + + static int ndesc_get_tx_ls(struct dma_desc *p) + { +- return (p->des1 & TDES1_LAST_SEGMENT) >> 30; ++ return (le32_to_cpu(p->des1) & TDES1_LAST_SEGMENT) >> 30; + } + + static void ndesc_release_tx_desc(struct dma_desc *p, int mode) + { +- int ter = (p->des1 & TDES1_END_RING) >> 25; ++ int ter = (le32_to_cpu(p->des1) & TDES1_END_RING) >> 25; + + memset(p, 0, offsetof(struct dma_desc, des2)); + if (mode == STMMAC_CHAIN_MODE) +@@ -197,7 +197,7 @@ static void ndesc_prepare_tx_desc(struct + bool csum_flag, int mode, bool tx_own, + bool ls) + { +- unsigned int tdes1 = p->des1; ++ unsigned int tdes1 = le32_to_cpu(p->des1); + + if (is_fs) + tdes1 |= TDES1_FIRST_SEGMENT; +@@ -212,7 +212,7 @@ static void ndesc_prepare_tx_desc(struct + if (ls) + tdes1 |= TDES1_LAST_SEGMENT; + +- p->des1 = tdes1; ++ p->des1 = cpu_to_le32(tdes1); + + if (mode == STMMAC_CHAIN_MODE) + norm_set_tx_desc_len_on_chain(p, len); +@@ -220,12 +220,12 @@ static void ndesc_prepare_tx_desc(struct + norm_set_tx_desc_len_on_ring(p, len); + + if (tx_own) +- p->des0 |= TDES0_OWN; ++ p->des0 |= cpu_to_le32(TDES0_OWN); + } + + static void ndesc_set_tx_ic(struct dma_desc *p) + { +- p->des1 |= TDES1_INTERRUPT; ++ p->des1 |= cpu_to_le32(TDES1_INTERRUPT); + } + + static int ndesc_get_rx_frame_len(struct dma_desc *p, int rx_coe_type) +@@ -241,19 +241,20 @@ static int ndesc_get_rx_frame_len(struct + if (rx_coe_type == STMMAC_RX_COE_TYPE1) + csum = 2; + +- return (((p->des0 & RDES0_FRAME_LEN_MASK) >> RDES0_FRAME_LEN_SHIFT) - ++ return (((le32_to_cpu(p->des0) & RDES0_FRAME_LEN_MASK) ++ >> RDES0_FRAME_LEN_SHIFT) - + csum); + + } + + static void ndesc_enable_tx_timestamp(struct dma_desc *p) + { +- p->des1 |= TDES1_TIME_STAMP_ENABLE; ++ p->des1 |= cpu_to_le32(TDES1_TIME_STAMP_ENABLE); + } + + static int ndesc_get_tx_timestamp_status(struct dma_desc *p) + { +- return (p->des0 & TDES0_TIME_STAMP_STATUS) >> 17; ++ return (le32_to_cpu(p->des0) & TDES0_TIME_STAMP_STATUS) >> 17; + } + + static u64 ndesc_get_timestamp(void *desc, u32 ats) +@@ -261,9 +262,9 @@ static u64 ndesc_get_timestamp(void *des + struct dma_desc *p = (struct dma_desc *)desc; + u64 ns; + +- ns = p->des2; ++ ns = le32_to_cpu(p->des2); + /* convert high/sec time stamp value to nanosecond */ +- ns += p->des3 * 1000000000ULL; ++ ns += le32_to_cpu(p->des3) * 1000000000ULL; + + return ns; + } +@@ -272,7 +273,8 @@ static int ndesc_get_rx_timestamp_status + { + struct dma_desc *p = (struct dma_desc *)desc; + +- if ((p->des2 == 0xffffffff) && (p->des3 == 0xffffffff)) ++ if ((le32_to_cpu(p->des2) == 0xffffffff) && ++ (le32_to_cpu(p->des3) == 0xffffffff)) + /* timestamp is corrupted, hence don't store it */ + return 0; + else +--- a/drivers/net/ethernet/stmicro/stmmac/ring_mode.c ++++ b/drivers/net/ethernet/stmicro/stmmac/ring_mode.c +@@ -34,7 +34,7 @@ static int stmmac_jumbo_frm(void *p, str + unsigned int entry = priv->cur_tx; + struct dma_desc *desc; + unsigned int nopaged_len = skb_headlen(skb); +- unsigned int bmax, len; ++ unsigned int bmax, len, des2; + + if (priv->extend_desc) + desc = (struct dma_desc *)(priv->dma_etx + entry); +@@ -50,16 +50,17 @@ static int stmmac_jumbo_frm(void *p, str + + if (nopaged_len > BUF_SIZE_8KiB) { + +- desc->des2 = dma_map_single(priv->device, skb->data, +- bmax, DMA_TO_DEVICE); +- if (dma_mapping_error(priv->device, desc->des2)) ++ des2 = dma_map_single(priv->device, skb->data, bmax, ++ DMA_TO_DEVICE); ++ desc->des2 = cpu_to_le32(des2); ++ if (dma_mapping_error(priv->device, des2)) + return -1; + +- priv->tx_skbuff_dma[entry].buf = desc->des2; ++ priv->tx_skbuff_dma[entry].buf = des2; + priv->tx_skbuff_dma[entry].len = bmax; + priv->tx_skbuff_dma[entry].is_jumbo = true; + +- desc->des3 = desc->des2 + BUF_SIZE_4KiB; ++ desc->des3 = cpu_to_le32(des2 + BUF_SIZE_4KiB); + priv->hw->desc->prepare_tx_desc(desc, 1, bmax, csum, + STMMAC_RING_MODE, 0, false); + priv->tx_skbuff[entry] = NULL; +@@ -70,26 +71,28 @@ static int stmmac_jumbo_frm(void *p, str + else + desc = priv->dma_tx + entry; + +- desc->des2 = dma_map_single(priv->device, skb->data + bmax, +- len, DMA_TO_DEVICE); +- if (dma_mapping_error(priv->device, desc->des2)) ++ des2 = dma_map_single(priv->device, skb->data + bmax, len, ++ DMA_TO_DEVICE); ++ desc->des2 = cpu_to_le32(des2); ++ if (dma_mapping_error(priv->device, des2)) + return -1; +- priv->tx_skbuff_dma[entry].buf = desc->des2; ++ priv->tx_skbuff_dma[entry].buf = des2; + priv->tx_skbuff_dma[entry].len = len; + priv->tx_skbuff_dma[entry].is_jumbo = true; + +- desc->des3 = desc->des2 + BUF_SIZE_4KiB; ++ desc->des3 = cpu_to_le32(des2 + BUF_SIZE_4KiB); + priv->hw->desc->prepare_tx_desc(desc, 0, len, csum, + STMMAC_RING_MODE, 1, true); + } else { +- desc->des2 = dma_map_single(priv->device, skb->data, +- nopaged_len, DMA_TO_DEVICE); +- if (dma_mapping_error(priv->device, desc->des2)) ++ des2 = dma_map_single(priv->device, skb->data, ++ nopaged_len, DMA_TO_DEVICE); ++ desc->des2 = cpu_to_le32(des2); ++ if (dma_mapping_error(priv->device, des2)) + return -1; +- priv->tx_skbuff_dma[entry].buf = desc->des2; ++ priv->tx_skbuff_dma[entry].buf = des2; + priv->tx_skbuff_dma[entry].len = nopaged_len; + priv->tx_skbuff_dma[entry].is_jumbo = true; +- desc->des3 = desc->des2 + BUF_SIZE_4KiB; ++ desc->des3 = cpu_to_le32(des2 + BUF_SIZE_4KiB); + priv->hw->desc->prepare_tx_desc(desc, 1, nopaged_len, csum, + STMMAC_RING_MODE, 0, true); + } +@@ -115,13 +118,13 @@ static void stmmac_refill_desc3(void *pr + + /* Fill DES3 in case of RING mode */ + if (priv->dma_buf_sz >= BUF_SIZE_8KiB) +- p->des3 = p->des2 + BUF_SIZE_8KiB; ++ p->des3 = cpu_to_le32(le32_to_cpu(p->des2) + BUF_SIZE_8KiB); + } + + /* In ring mode we need to fill the desc3 because it is used as buffer */ + static void stmmac_init_desc3(struct dma_desc *p) + { +- p->des3 = p->des2 + BUF_SIZE_8KiB; ++ p->des3 = cpu_to_le32(le32_to_cpu(p->des2) + BUF_SIZE_8KiB); + } + + static void stmmac_clean_desc3(void *priv_ptr, struct dma_desc *p) +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h +@@ -64,7 +64,6 @@ struct stmmac_priv { + dma_addr_t dma_tx_phy; + int tx_coalesce; + int hwts_tx_en; +- spinlock_t tx_lock; + bool tx_path_in_lpi_mode; + struct timer_list txtimer; + bool tso; +@@ -90,7 +89,6 @@ struct stmmac_priv { + struct mac_device_info *hw; + spinlock_t lock; + +- struct phy_device *phydev ____cacheline_aligned_in_smp; + int oldlink; + int speed; + int oldduplex; +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c +@@ -263,7 +263,7 @@ static void stmmac_ethtool_getdrvinfo(st + { + struct stmmac_priv *priv = netdev_priv(dev); + +- if (priv->plat->has_gmac) ++ if (priv->plat->has_gmac || priv->plat->has_gmac4) + strlcpy(info->driver, GMAC_ETHTOOL_NAME, sizeof(info->driver)); + else + strlcpy(info->driver, MAC100_ETHTOOL_NAME, +@@ -272,25 +272,26 @@ static void stmmac_ethtool_getdrvinfo(st + strlcpy(info->version, DRV_MODULE_VERSION, sizeof(info->version)); + } + +-static int stmmac_ethtool_getsettings(struct net_device *dev, +- struct ethtool_cmd *cmd) ++static int stmmac_ethtool_get_link_ksettings(struct net_device *dev, ++ struct ethtool_link_ksettings *cmd) + { + struct stmmac_priv *priv = netdev_priv(dev); +- struct phy_device *phy = priv->phydev; ++ struct phy_device *phy = dev->phydev; + int rc; + + if (priv->hw->pcs & STMMAC_PCS_RGMII || + priv->hw->pcs & STMMAC_PCS_SGMII) { + struct rgmii_adv adv; ++ u32 supported, advertising, lp_advertising; + + if (!priv->xstats.pcs_link) { +- ethtool_cmd_speed_set(cmd, SPEED_UNKNOWN); +- cmd->duplex = DUPLEX_UNKNOWN; ++ cmd->base.speed = SPEED_UNKNOWN; ++ cmd->base.duplex = DUPLEX_UNKNOWN; + return 0; + } +- cmd->duplex = priv->xstats.pcs_duplex; ++ cmd->base.duplex = priv->xstats.pcs_duplex; + +- ethtool_cmd_speed_set(cmd, priv->xstats.pcs_speed); ++ cmd->base.speed = priv->xstats.pcs_speed; + + /* Get and convert ADV/LP_ADV from the HW AN registers */ + if (!priv->hw->mac->pcs_get_adv_lp) +@@ -300,45 +301,59 @@ static int stmmac_ethtool_getsettings(st + + /* Encoding of PSE bits is defined in 802.3z, 37.2.1.4 */ + ++ ethtool_convert_link_mode_to_legacy_u32( ++ &supported, cmd->link_modes.supported); ++ ethtool_convert_link_mode_to_legacy_u32( ++ &advertising, cmd->link_modes.advertising); ++ ethtool_convert_link_mode_to_legacy_u32( ++ &lp_advertising, cmd->link_modes.lp_advertising); ++ + if (adv.pause & STMMAC_PCS_PAUSE) +- cmd->advertising |= ADVERTISED_Pause; ++ advertising |= ADVERTISED_Pause; + if (adv.pause & STMMAC_PCS_ASYM_PAUSE) +- cmd->advertising |= ADVERTISED_Asym_Pause; ++ advertising |= ADVERTISED_Asym_Pause; + if (adv.lp_pause & STMMAC_PCS_PAUSE) +- cmd->lp_advertising |= ADVERTISED_Pause; ++ lp_advertising |= ADVERTISED_Pause; + if (adv.lp_pause & STMMAC_PCS_ASYM_PAUSE) +- cmd->lp_advertising |= ADVERTISED_Asym_Pause; ++ lp_advertising |= ADVERTISED_Asym_Pause; + + /* Reg49[3] always set because ANE is always supported */ +- cmd->autoneg = ADVERTISED_Autoneg; +- cmd->supported |= SUPPORTED_Autoneg; +- cmd->advertising |= ADVERTISED_Autoneg; +- cmd->lp_advertising |= ADVERTISED_Autoneg; ++ cmd->base.autoneg = ADVERTISED_Autoneg; ++ supported |= SUPPORTED_Autoneg; ++ advertising |= ADVERTISED_Autoneg; ++ lp_advertising |= ADVERTISED_Autoneg; + + if (adv.duplex) { +- cmd->supported |= (SUPPORTED_1000baseT_Full | +- SUPPORTED_100baseT_Full | +- SUPPORTED_10baseT_Full); +- cmd->advertising |= (ADVERTISED_1000baseT_Full | +- ADVERTISED_100baseT_Full | +- ADVERTISED_10baseT_Full); ++ supported |= (SUPPORTED_1000baseT_Full | ++ SUPPORTED_100baseT_Full | ++ SUPPORTED_10baseT_Full); ++ advertising |= (ADVERTISED_1000baseT_Full | ++ ADVERTISED_100baseT_Full | ++ ADVERTISED_10baseT_Full); + } else { +- cmd->supported |= (SUPPORTED_1000baseT_Half | +- SUPPORTED_100baseT_Half | +- SUPPORTED_10baseT_Half); +- cmd->advertising |= (ADVERTISED_1000baseT_Half | +- ADVERTISED_100baseT_Half | +- ADVERTISED_10baseT_Half); ++ supported |= (SUPPORTED_1000baseT_Half | ++ SUPPORTED_100baseT_Half | ++ SUPPORTED_10baseT_Half); ++ advertising |= (ADVERTISED_1000baseT_Half | ++ ADVERTISED_100baseT_Half | ++ ADVERTISED_10baseT_Half); + } + if (adv.lp_duplex) +- cmd->lp_advertising |= (ADVERTISED_1000baseT_Full | +- ADVERTISED_100baseT_Full | +- ADVERTISED_10baseT_Full); ++ lp_advertising |= (ADVERTISED_1000baseT_Full | ++ ADVERTISED_100baseT_Full | ++ ADVERTISED_10baseT_Full); + else +- cmd->lp_advertising |= (ADVERTISED_1000baseT_Half | +- ADVERTISED_100baseT_Half | +- ADVERTISED_10baseT_Half); +- cmd->port = PORT_OTHER; ++ lp_advertising |= (ADVERTISED_1000baseT_Half | ++ ADVERTISED_100baseT_Half | ++ ADVERTISED_10baseT_Half); ++ cmd->base.port = PORT_OTHER; ++ ++ ethtool_convert_legacy_u32_to_link_mode( ++ cmd->link_modes.supported, supported); ++ ethtool_convert_legacy_u32_to_link_mode( ++ cmd->link_modes.advertising, advertising); ++ ethtool_convert_legacy_u32_to_link_mode( ++ cmd->link_modes.lp_advertising, lp_advertising); + + return 0; + } +@@ -353,16 +368,16 @@ static int stmmac_ethtool_getsettings(st + "link speed / duplex setting\n", dev->name); + return -EBUSY; + } +- cmd->transceiver = XCVR_INTERNAL; +- rc = phy_ethtool_gset(phy, cmd); ++ rc = phy_ethtool_ksettings_get(phy, cmd); + return rc; + } + +-static int stmmac_ethtool_setsettings(struct net_device *dev, +- struct ethtool_cmd *cmd) ++static int ++stmmac_ethtool_set_link_ksettings(struct net_device *dev, ++ const struct ethtool_link_ksettings *cmd) + { + struct stmmac_priv *priv = netdev_priv(dev); +- struct phy_device *phy = priv->phydev; ++ struct phy_device *phy = dev->phydev; + int rc; + + if (priv->hw->pcs & STMMAC_PCS_RGMII || +@@ -370,7 +385,7 @@ static int stmmac_ethtool_setsettings(st + u32 mask = ADVERTISED_Autoneg | ADVERTISED_Pause; + + /* Only support ANE */ +- if (cmd->autoneg != AUTONEG_ENABLE) ++ if (cmd->base.autoneg != AUTONEG_ENABLE) + return -EINVAL; + + mask &= (ADVERTISED_1000baseT_Half | +@@ -391,9 +406,7 @@ static int stmmac_ethtool_setsettings(st + return 0; + } + +- spin_lock(&priv->lock); +- rc = phy_ethtool_sset(phy, cmd); +- spin_unlock(&priv->lock); ++ rc = phy_ethtool_ksettings_set(phy, cmd); + + return rc; + } +@@ -433,7 +446,7 @@ static void stmmac_ethtool_gregs(struct + + memset(reg_space, 0x0, REG_SPACE_SIZE); + +- if (!priv->plat->has_gmac) { ++ if (!(priv->plat->has_gmac || priv->plat->has_gmac4)) { + /* MAC registers */ + for (i = 0; i < 12; i++) + reg_space[i] = readl(priv->ioaddr + (i * 4)); +@@ -471,12 +484,12 @@ stmmac_get_pauseparam(struct net_device + if (!adv_lp.pause) + return; + } else { +- if (!(priv->phydev->supported & SUPPORTED_Pause) || +- !(priv->phydev->supported & SUPPORTED_Asym_Pause)) ++ if (!(netdev->phydev->supported & SUPPORTED_Pause) || ++ !(netdev->phydev->supported & SUPPORTED_Asym_Pause)) + return; + } + +- pause->autoneg = priv->phydev->autoneg; ++ pause->autoneg = netdev->phydev->autoneg; + + if (priv->flow_ctrl & FLOW_RX) + pause->rx_pause = 1; +@@ -490,7 +503,7 @@ stmmac_set_pauseparam(struct net_device + struct ethtool_pauseparam *pause) + { + struct stmmac_priv *priv = netdev_priv(netdev); +- struct phy_device *phy = priv->phydev; ++ struct phy_device *phy = netdev->phydev; + int new_pause = FLOW_OFF; + + if (priv->hw->pcs && priv->hw->mac->pcs_get_adv_lp) { +@@ -550,7 +563,7 @@ static void stmmac_get_ethtool_stats(str + } + } + if (priv->eee_enabled) { +- int val = phy_get_eee_err(priv->phydev); ++ int val = phy_get_eee_err(dev->phydev); + if (val) + priv->xstats.phy_eee_wakeup_error_n = val; + } +@@ -669,7 +682,7 @@ static int stmmac_ethtool_op_get_eee(str + edata->eee_active = priv->eee_active; + edata->tx_lpi_timer = priv->tx_lpi_timer; + +- return phy_ethtool_get_eee(priv->phydev, edata); ++ return phy_ethtool_get_eee(dev->phydev, edata); + } + + static int stmmac_ethtool_op_set_eee(struct net_device *dev, +@@ -694,7 +707,7 @@ static int stmmac_ethtool_op_set_eee(str + priv->tx_lpi_timer = edata->tx_lpi_timer; + } + +- return phy_ethtool_set_eee(priv->phydev, edata); ++ return phy_ethtool_set_eee(dev->phydev, edata); + } + + static u32 stmmac_usec2riwt(u32 usec, struct stmmac_priv *priv) +@@ -853,8 +866,6 @@ static int stmmac_set_tunable(struct net + static const struct ethtool_ops stmmac_ethtool_ops = { + .begin = stmmac_check_if_running, + .get_drvinfo = stmmac_ethtool_getdrvinfo, +- .get_settings = stmmac_ethtool_getsettings, +- .set_settings = stmmac_ethtool_setsettings, + .get_msglevel = stmmac_ethtool_getmsglevel, + .set_msglevel = stmmac_ethtool_setmsglevel, + .get_regs = stmmac_ethtool_gregs, +@@ -874,6 +885,8 @@ static const struct ethtool_ops stmmac_e + .set_coalesce = stmmac_set_coalesce, + .get_tunable = stmmac_get_tunable, + .set_tunable = stmmac_set_tunable, ++ .get_link_ksettings = stmmac_ethtool_get_link_ksettings, ++ .set_link_ksettings = stmmac_ethtool_set_link_ksettings, + }; + + void stmmac_set_ethtool_ops(struct net_device *netdev) +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +@@ -105,8 +105,8 @@ module_param(eee_timer, int, S_IRUGO | S + MODULE_PARM_DESC(eee_timer, "LPI tx expiration time in msec"); + #define STMMAC_LPI_T(x) (jiffies + msecs_to_jiffies(x)) + +-/* By default the driver will use the ring mode to manage tx and rx descriptors +- * but passing this value so user can force to use the chain instead of the ring ++/* By default the driver will use the ring mode to manage tx and rx descriptors, ++ * but allow user to force to use the chain instead of the ring + */ + static unsigned int chain_mode; + module_param(chain_mode, int, S_IRUGO); +@@ -221,7 +221,8 @@ static inline u32 stmmac_rx_dirty(struct + */ + static inline void stmmac_hw_fix_mac_speed(struct stmmac_priv *priv) + { +- struct phy_device *phydev = priv->phydev; ++ struct net_device *ndev = priv->dev; ++ struct phy_device *phydev = ndev->phydev; + + if (likely(priv->plat->fix_mac_speed)) + priv->plat->fix_mac_speed(priv->plat->bsp_priv, phydev->speed); +@@ -279,6 +280,7 @@ static void stmmac_eee_ctrl_timer(unsign + */ + bool stmmac_eee_init(struct stmmac_priv *priv) + { ++ struct net_device *ndev = priv->dev; + unsigned long flags; + bool ret = false; + +@@ -295,7 +297,7 @@ bool stmmac_eee_init(struct stmmac_priv + int tx_lpi_timer = priv->tx_lpi_timer; + + /* Check if the PHY supports EEE */ +- if (phy_init_eee(priv->phydev, 1)) { ++ if (phy_init_eee(ndev->phydev, 1)) { + /* To manage at run-time if the EEE cannot be supported + * anymore (for example because the lp caps have been + * changed). +@@ -303,7 +305,7 @@ bool stmmac_eee_init(struct stmmac_priv + */ + spin_lock_irqsave(&priv->lock, flags); + if (priv->eee_active) { +- pr_debug("stmmac: disable EEE\n"); ++ netdev_dbg(priv->dev, "disable EEE\n"); + del_timer_sync(&priv->eee_ctrl_timer); + priv->hw->mac->set_eee_timer(priv->hw, 0, + tx_lpi_timer); +@@ -327,12 +329,12 @@ bool stmmac_eee_init(struct stmmac_priv + tx_lpi_timer); + } + /* Set HW EEE according to the speed */ +- priv->hw->mac->set_eee_pls(priv->hw, priv->phydev->link); ++ priv->hw->mac->set_eee_pls(priv->hw, ndev->phydev->link); + + ret = true; + spin_unlock_irqrestore(&priv->lock, flags); + +- pr_debug("stmmac: Energy-Efficient Ethernet initialized\n"); ++ netdev_dbg(priv->dev, "Energy-Efficient Ethernet initialized\n"); + } + out: + return ret; +@@ -450,8 +452,8 @@ static int stmmac_hwtstamp_ioctl(struct + sizeof(struct hwtstamp_config))) + return -EFAULT; + +- pr_debug("%s config flags:0x%x, tx_type:0x%x, rx_filter:0x%x\n", +- __func__, config.flags, config.tx_type, config.rx_filter); ++ netdev_dbg(priv->dev, "%s config flags:0x%x, tx_type:0x%x, rx_filter:0x%x\n", ++ __func__, config.flags, config.tx_type, config.rx_filter); + + /* reserved for future extensions */ + if (config.flags) +@@ -697,7 +699,7 @@ static void stmmac_release_ptp(struct st + static void stmmac_adjust_link(struct net_device *dev) + { + struct stmmac_priv *priv = netdev_priv(dev); +- struct phy_device *phydev = priv->phydev; ++ struct phy_device *phydev = dev->phydev; + unsigned long flags; + int new_state = 0; + unsigned int fc = priv->flow_ctrl, pause_time = priv->pause; +@@ -750,9 +752,9 @@ static void stmmac_adjust_link(struct ne + stmmac_hw_fix_mac_speed(priv); + break; + default: +- if (netif_msg_link(priv)) +- pr_warn("%s: Speed (%d) not 10/100\n", +- dev->name, phydev->speed); ++ netif_warn(priv, link, priv->dev, ++ "Speed (%d) not 10/100\n", ++ phydev->speed); + break; + } + +@@ -805,10 +807,10 @@ static void stmmac_check_pcs_mode(struct + (interface == PHY_INTERFACE_MODE_RGMII_ID) || + (interface == PHY_INTERFACE_MODE_RGMII_RXID) || + (interface == PHY_INTERFACE_MODE_RGMII_TXID)) { +- pr_debug("STMMAC: PCS RGMII support enable\n"); ++ netdev_dbg(priv->dev, "PCS RGMII support enabled\n"); + priv->hw->pcs = STMMAC_PCS_RGMII; + } else if (interface == PHY_INTERFACE_MODE_SGMII) { +- pr_debug("STMMAC: PCS SGMII support enable\n"); ++ netdev_dbg(priv->dev, "PCS SGMII support enabled\n"); + priv->hw->pcs = STMMAC_PCS_SGMII; + } + } +@@ -843,15 +845,15 @@ static int stmmac_init_phy(struct net_de + + snprintf(phy_id_fmt, MII_BUS_ID_SIZE + 3, PHY_ID_FMT, bus_id, + priv->plat->phy_addr); +- pr_debug("stmmac_init_phy: trying to attach to %s\n", +- phy_id_fmt); ++ netdev_dbg(priv->dev, "%s: trying to attach to %s\n", __func__, ++ phy_id_fmt); + + phydev = phy_connect(dev, phy_id_fmt, &stmmac_adjust_link, + interface); + } + + if (IS_ERR_OR_NULL(phydev)) { +- pr_err("%s: Could not attach to PHY\n", dev->name); ++ netdev_err(priv->dev, "Could not attach to PHY\n"); + if (!phydev) + return -ENODEV; + +@@ -884,10 +886,8 @@ static int stmmac_init_phy(struct net_de + if (phydev->is_pseudo_fixed_link) + phydev->irq = PHY_POLL; + +- pr_debug("stmmac_init_phy: %s: attached to PHY (UID 0x%x)" +- " Link = %d\n", dev->name, phydev->phy_id, phydev->link); +- +- priv->phydev = phydev; ++ netdev_dbg(priv->dev, "%s: attached to PHY (UID 0x%x) Link = %d\n", ++ __func__, phydev->phy_id, phydev->link); + + return 0; + } +@@ -973,7 +973,8 @@ static int stmmac_init_rx_buffers(struct + + skb = __netdev_alloc_skb_ip_align(priv->dev, priv->dma_buf_sz, flags); + if (!skb) { +- pr_err("%s: Rx init fails; skb is NULL\n", __func__); ++ netdev_err(priv->dev, ++ "%s: Rx init fails; skb is NULL\n", __func__); + return -ENOMEM; + } + priv->rx_skbuff[i] = skb; +@@ -981,15 +982,15 @@ static int stmmac_init_rx_buffers(struct + priv->dma_buf_sz, + DMA_FROM_DEVICE); + if (dma_mapping_error(priv->device, priv->rx_skbuff_dma[i])) { +- pr_err("%s: DMA mapping error\n", __func__); ++ netdev_err(priv->dev, "%s: DMA mapping error\n", __func__); + dev_kfree_skb_any(skb); + return -EINVAL; + } + + if (priv->synopsys_id >= DWMAC_CORE_4_00) +- p->des0 = priv->rx_skbuff_dma[i]; ++ p->des0 = cpu_to_le32(priv->rx_skbuff_dma[i]); + else +- p->des2 = priv->rx_skbuff_dma[i]; ++ p->des2 = cpu_to_le32(priv->rx_skbuff_dma[i]); + + if ((priv->hw->mode->init_desc3) && + (priv->dma_buf_sz == BUF_SIZE_16KiB)) +@@ -1031,13 +1032,14 @@ static int init_dma_desc_rings(struct ne + + priv->dma_buf_sz = bfsize; + +- if (netif_msg_probe(priv)) { +- pr_debug("(%s) dma_rx_phy=0x%08x dma_tx_phy=0x%08x\n", __func__, +- (u32) priv->dma_rx_phy, (u32) priv->dma_tx_phy); ++ netif_dbg(priv, probe, priv->dev, ++ "(%s) dma_rx_phy=0x%08x dma_tx_phy=0x%08x\n", ++ __func__, (u32)priv->dma_rx_phy, (u32)priv->dma_tx_phy); ++ ++ /* RX INITIALIZATION */ ++ netif_dbg(priv, probe, priv->dev, ++ "SKB addresses:\nskb\t\tskb data\tdma data\n"); + +- /* RX INITIALIZATION */ +- pr_debug("\tSKB addresses:\nskb\t\tskb data\tdma data\n"); +- } + for (i = 0; i < DMA_RX_SIZE; i++) { + struct dma_desc *p; + if (priv->extend_desc) +@@ -1049,10 +1051,9 @@ static int init_dma_desc_rings(struct ne + if (ret) + goto err_init_rx_buffers; + +- if (netif_msg_probe(priv)) +- pr_debug("[%p]\t[%p]\t[%x]\n", priv->rx_skbuff[i], +- priv->rx_skbuff[i]->data, +- (unsigned int)priv->rx_skbuff_dma[i]); ++ netif_dbg(priv, probe, priv->dev, "[%p]\t[%p]\t[%x]\n", ++ priv->rx_skbuff[i], priv->rx_skbuff[i]->data, ++ (unsigned int)priv->rx_skbuff_dma[i]); + } + priv->cur_rx = 0; + priv->dirty_rx = (unsigned int)(i - DMA_RX_SIZE); +@@ -1307,7 +1308,7 @@ static void stmmac_tx_clean(struct stmma + unsigned int bytes_compl = 0, pkts_compl = 0; + unsigned int entry = priv->dirty_tx; + +- spin_lock(&priv->tx_lock); ++ netif_tx_lock(priv->dev); + + priv->xstats.tx_clean++; + +@@ -1378,22 +1379,17 @@ static void stmmac_tx_clean(struct stmma + netdev_completed_queue(priv->dev, pkts_compl, bytes_compl); + + if (unlikely(netif_queue_stopped(priv->dev) && +- stmmac_tx_avail(priv) > STMMAC_TX_THRESH)) { +- netif_tx_lock(priv->dev); +- if (netif_queue_stopped(priv->dev) && +- stmmac_tx_avail(priv) > STMMAC_TX_THRESH) { +- if (netif_msg_tx_done(priv)) +- pr_debug("%s: restart transmit\n", __func__); +- netif_wake_queue(priv->dev); +- } +- netif_tx_unlock(priv->dev); ++ stmmac_tx_avail(priv) > STMMAC_TX_THRESH)) { ++ netif_dbg(priv, tx_done, priv->dev, ++ "%s: restart transmit\n", __func__); ++ netif_wake_queue(priv->dev); + } + + if ((priv->eee_enabled) && (!priv->tx_path_in_lpi_mode)) { + stmmac_enable_eee_mode(priv); + mod_timer(&priv->eee_ctrl_timer, STMMAC_LPI_T(eee_timer)); + } +- spin_unlock(&priv->tx_lock); ++ netif_tx_unlock(priv->dev); + } + + static inline void stmmac_enable_dma_irq(struct stmmac_priv *priv) +@@ -1497,7 +1493,7 @@ static void stmmac_mmc_setup(struct stmm + dwmac_mmc_ctrl(priv->mmcaddr, mode); + memset(&priv->mmc, 0, sizeof(struct stmmac_counters)); + } else +- pr_info(" No MAC Management Counters available\n"); ++ netdev_info(priv->dev, "No MAC Management Counters available\n"); + } + + /** +@@ -1510,18 +1506,18 @@ static void stmmac_mmc_setup(struct stmm + static void stmmac_selec_desc_mode(struct stmmac_priv *priv) + { + if (priv->plat->enh_desc) { +- pr_info(" Enhanced/Alternate descriptors\n"); ++ dev_info(priv->device, "Enhanced/Alternate descriptors\n"); + + /* GMAC older than 3.50 has no extended descriptors */ + if (priv->synopsys_id >= DWMAC_CORE_3_50) { +- pr_info("\tEnabled extended descriptors\n"); ++ dev_info(priv->device, "Enabled extended descriptors\n"); + priv->extend_desc = 1; + } else +- pr_warn("Extended descriptors not supported\n"); ++ dev_warn(priv->device, "Extended descriptors not supported\n"); + + priv->hw->desc = &enh_desc_ops; + } else { +- pr_info(" Normal descriptors\n"); ++ dev_info(priv->device, "Normal descriptors\n"); + priv->hw->desc = &ndesc_ops; + } + } +@@ -1562,8 +1558,8 @@ static void stmmac_check_ether_addr(stru + priv->dev->dev_addr, 0); + if (!is_valid_ether_addr(priv->dev->dev_addr)) + eth_hw_addr_random(priv->dev); +- pr_info("%s: device MAC address %pM\n", priv->dev->name, +- priv->dev->dev_addr); ++ netdev_info(priv->dev, "device MAC address %pM\n", ++ priv->dev->dev_addr); + } + } + +@@ -1577,16 +1573,12 @@ static void stmmac_check_ether_addr(stru + */ + static int stmmac_init_dma_engine(struct stmmac_priv *priv) + { +- int pbl = DEFAULT_DMA_PBL, fixed_burst = 0, aal = 0; +- int mixed_burst = 0; + int atds = 0; + int ret = 0; + +- if (priv->plat->dma_cfg) { +- pbl = priv->plat->dma_cfg->pbl; +- fixed_burst = priv->plat->dma_cfg->fixed_burst; +- mixed_burst = priv->plat->dma_cfg->mixed_burst; +- aal = priv->plat->dma_cfg->aal; ++ if (!priv->plat->dma_cfg || !priv->plat->dma_cfg->pbl) { ++ dev_err(priv->device, "Invalid DMA configuration\n"); ++ return -EINVAL; + } + + if (priv->extend_desc && (priv->mode == STMMAC_RING_MODE)) +@@ -1598,8 +1590,8 @@ static int stmmac_init_dma_engine(struct + return ret; + } + +- priv->hw->dma->init(priv->ioaddr, pbl, fixed_burst, mixed_burst, +- aal, priv->dma_tx_phy, priv->dma_rx_phy, atds); ++ priv->hw->dma->init(priv->ioaddr, priv->plat->dma_cfg, ++ priv->dma_tx_phy, priv->dma_rx_phy, atds); + + if (priv->synopsys_id >= DWMAC_CORE_4_00) { + priv->rx_tail_addr = priv->dma_rx_phy + +@@ -1671,7 +1663,8 @@ static int stmmac_hw_setup(struct net_de + /* DMA initialization and SW reset */ + ret = stmmac_init_dma_engine(priv); + if (ret < 0) { +- pr_err("%s: DMA engine initialization failed\n", __func__); ++ netdev_err(priv->dev, "%s: DMA engine initialization failed\n", ++ __func__); + return ret; + } + +@@ -1700,7 +1693,7 @@ static int stmmac_hw_setup(struct net_de + + ret = priv->hw->mac->rx_ipc(priv->hw); + if (!ret) { +- pr_warn(" RX IPC Checksum Offload disabled\n"); ++ netdev_warn(priv->dev, "RX IPC Checksum Offload disabled\n"); + priv->plat->rx_coe = STMMAC_RX_COE_NONE; + priv->hw->rx_csum = 0; + } +@@ -1725,10 +1718,11 @@ static int stmmac_hw_setup(struct net_de + #ifdef CONFIG_DEBUG_FS + ret = stmmac_init_fs(dev); + if (ret < 0) +- pr_warn("%s: failed debugFS registration\n", __func__); ++ netdev_warn(priv->dev, "%s: failed debugFS registration\n", ++ __func__); + #endif + /* Start the ball rolling... */ +- pr_debug("%s: DMA RX/TX processes started...\n", dev->name); ++ netdev_dbg(priv->dev, "DMA RX/TX processes started...\n"); + priv->hw->dma->start_tx(priv->ioaddr); + priv->hw->dma->start_rx(priv->ioaddr); + +@@ -1783,8 +1777,9 @@ static int stmmac_open(struct net_device + priv->hw->pcs != STMMAC_PCS_RTBI) { + ret = stmmac_init_phy(dev); + if (ret) { +- pr_err("%s: Cannot attach to PHY (error: %d)\n", +- __func__, ret); ++ netdev_err(priv->dev, ++ "%s: Cannot attach to PHY (error: %d)\n", ++ __func__, ret); + return ret; + } + } +@@ -1798,33 +1793,36 @@ static int stmmac_open(struct net_device + + ret = alloc_dma_desc_resources(priv); + if (ret < 0) { +- pr_err("%s: DMA descriptors allocation failed\n", __func__); ++ netdev_err(priv->dev, "%s: DMA descriptors allocation failed\n", ++ __func__); + goto dma_desc_error; + } + + ret = init_dma_desc_rings(dev, GFP_KERNEL); + if (ret < 0) { +- pr_err("%s: DMA descriptors initialization failed\n", __func__); ++ netdev_err(priv->dev, "%s: DMA descriptors initialization failed\n", ++ __func__); + goto init_error; + } + + ret = stmmac_hw_setup(dev, true); + if (ret < 0) { +- pr_err("%s: Hw setup failed\n", __func__); ++ netdev_err(priv->dev, "%s: Hw setup failed\n", __func__); + goto init_error; + } + + stmmac_init_tx_coalesce(priv); + +- if (priv->phydev) +- phy_start(priv->phydev); ++ if (dev->phydev) ++ phy_start(dev->phydev); + + /* Request the IRQ lines */ + ret = request_irq(dev->irq, stmmac_interrupt, + IRQF_SHARED, dev->name, dev); + if (unlikely(ret < 0)) { +- pr_err("%s: ERROR: allocating the IRQ %d (error: %d)\n", +- __func__, dev->irq, ret); ++ netdev_err(priv->dev, ++ "%s: ERROR: allocating the IRQ %d (error: %d)\n", ++ __func__, dev->irq, ret); + goto init_error; + } + +@@ -1833,8 +1831,9 @@ static int stmmac_open(struct net_device + ret = request_irq(priv->wol_irq, stmmac_interrupt, + IRQF_SHARED, dev->name, dev); + if (unlikely(ret < 0)) { +- pr_err("%s: ERROR: allocating the WoL IRQ %d (%d)\n", +- __func__, priv->wol_irq, ret); ++ netdev_err(priv->dev, ++ "%s: ERROR: allocating the WoL IRQ %d (%d)\n", ++ __func__, priv->wol_irq, ret); + goto wolirq_error; + } + } +@@ -1844,8 +1843,9 @@ static int stmmac_open(struct net_device + ret = request_irq(priv->lpi_irq, stmmac_interrupt, IRQF_SHARED, + dev->name, dev); + if (unlikely(ret < 0)) { +- pr_err("%s: ERROR: allocating the LPI IRQ %d (%d)\n", +- __func__, priv->lpi_irq, ret); ++ netdev_err(priv->dev, ++ "%s: ERROR: allocating the LPI IRQ %d (%d)\n", ++ __func__, priv->lpi_irq, ret); + goto lpiirq_error; + } + } +@@ -1864,8 +1864,8 @@ wolirq_error: + init_error: + free_dma_desc_resources(priv); + dma_desc_error: +- if (priv->phydev) +- phy_disconnect(priv->phydev); ++ if (dev->phydev) ++ phy_disconnect(dev->phydev); + + return ret; + } +@@ -1884,10 +1884,9 @@ static int stmmac_release(struct net_dev + del_timer_sync(&priv->eee_ctrl_timer); + + /* Stop and disconnect the PHY */ +- if (priv->phydev) { +- phy_stop(priv->phydev); +- phy_disconnect(priv->phydev); +- priv->phydev = NULL; ++ if (dev->phydev) { ++ phy_stop(dev->phydev); ++ phy_disconnect(dev->phydev); + } + + netif_stop_queue(dev); +@@ -1947,13 +1946,13 @@ static void stmmac_tso_allocator(struct + priv->cur_tx = STMMAC_GET_ENTRY(priv->cur_tx, DMA_TX_SIZE); + desc = priv->dma_tx + priv->cur_tx; + +- desc->des0 = des + (total_len - tmp_len); ++ desc->des0 = cpu_to_le32(des + (total_len - tmp_len)); + buff_size = tmp_len >= TSO_MAX_BUFF_SIZE ? + TSO_MAX_BUFF_SIZE : tmp_len; + + priv->hw->desc->prepare_tso_tx_desc(desc, 0, buff_size, + 0, 1, +- (last_segment) && (tmp_len <= TSO_MAX_BUFF_SIZE), ++ (last_segment) && (buff_size < TSO_MAX_BUFF_SIZE), + 0, 0); + + tmp_len -= TSO_MAX_BUFF_SIZE; +@@ -1998,8 +1997,6 @@ static netdev_tx_t stmmac_tso_xmit(struc + u8 proto_hdr_len; + int i; + +- spin_lock(&priv->tx_lock); +- + /* Compute header lengths */ + proto_hdr_len = skb_transport_offset(skb) + tcp_hdrlen(skb); + +@@ -2009,9 +2006,10 @@ static netdev_tx_t stmmac_tso_xmit(struc + if (!netif_queue_stopped(dev)) { + netif_stop_queue(dev); + /* This is a hard error, log it. */ +- pr_err("%s: Tx Ring full when queue awake\n", __func__); ++ netdev_err(priv->dev, ++ "%s: Tx Ring full when queue awake\n", ++ __func__); + } +- spin_unlock(&priv->tx_lock); + return NETDEV_TX_BUSY; + } + +@@ -2049,11 +2047,11 @@ static netdev_tx_t stmmac_tso_xmit(struc + priv->tx_skbuff_dma[first_entry].len = skb_headlen(skb); + priv->tx_skbuff[first_entry] = skb; + +- first->des0 = des; ++ first->des0 = cpu_to_le32(des); + + /* Fill start of payload in buff2 of first descriptor */ + if (pay_len) +- first->des1 = des + proto_hdr_len; ++ first->des1 = cpu_to_le32(des + proto_hdr_len); + + /* If needed take extra descriptors to fill the remaining payload */ + tmp_pay_len = pay_len - TSO_MAX_BUFF_SIZE; +@@ -2082,8 +2080,8 @@ static netdev_tx_t stmmac_tso_xmit(struc + priv->cur_tx = STMMAC_GET_ENTRY(priv->cur_tx, DMA_TX_SIZE); + + if (unlikely(stmmac_tx_avail(priv) <= (MAX_SKB_FRAGS + 1))) { +- if (netif_msg_hw(priv)) +- pr_debug("%s: stop transmitted packets\n", __func__); ++ netif_dbg(priv, hw, priv->dev, "%s: stop transmitted packets\n", ++ __func__); + netif_stop_queue(dev); + } + +@@ -2127,7 +2125,7 @@ static netdev_tx_t stmmac_tso_xmit(struc + * descriptor and then barrier is needed to make sure that + * all is coherent before granting the DMA engine. + */ +- smp_wmb(); ++ dma_wmb(); + + if (netif_msg_pktdata(priv)) { + pr_info("%s: curr=%d dirty=%d f=%d, e=%d, f_p=%p, nfrags %d\n", +@@ -2146,11 +2144,9 @@ static netdev_tx_t stmmac_tso_xmit(struc + priv->hw->dma->set_tx_tail_ptr(priv->ioaddr, priv->tx_tail_addr, + STMMAC_CHAN0); + +- spin_unlock(&priv->tx_lock); + return NETDEV_TX_OK; + + dma_map_err: +- spin_unlock(&priv->tx_lock); + dev_err(priv->device, "Tx dma map failed\n"); + dev_kfree_skb(skb); + priv->dev->stats.tx_dropped++; +@@ -2182,14 +2178,13 @@ static netdev_tx_t stmmac_xmit(struct sk + return stmmac_tso_xmit(skb, dev); + } + +- spin_lock(&priv->tx_lock); +- + if (unlikely(stmmac_tx_avail(priv) < nfrags + 1)) { +- spin_unlock(&priv->tx_lock); + if (!netif_queue_stopped(dev)) { + netif_stop_queue(dev); + /* This is a hard error, log it. */ +- pr_err("%s: Tx Ring full when queue awake\n", __func__); ++ netdev_err(priv->dev, ++ "%s: Tx Ring full when queue awake\n", ++ __func__); + } + return NETDEV_TX_BUSY; + } +@@ -2242,13 +2237,11 @@ static netdev_tx_t stmmac_xmit(struct sk + + priv->tx_skbuff[entry] = NULL; + +- if (unlikely(priv->synopsys_id >= DWMAC_CORE_4_00)) { +- desc->des0 = des; +- priv->tx_skbuff_dma[entry].buf = desc->des0; +- } else { +- desc->des2 = des; +- priv->tx_skbuff_dma[entry].buf = desc->des2; +- } ++ priv->tx_skbuff_dma[entry].buf = des; ++ if (unlikely(priv->synopsys_id >= DWMAC_CORE_4_00)) ++ desc->des0 = cpu_to_le32(des); ++ else ++ desc->des2 = cpu_to_le32(des); + + priv->tx_skbuff_dma[entry].map_as_page = true; + priv->tx_skbuff_dma[entry].len = len; +@@ -2266,9 +2259,10 @@ static netdev_tx_t stmmac_xmit(struct sk + if (netif_msg_pktdata(priv)) { + void *tx_head; + +- pr_debug("%s: curr=%d dirty=%d f=%d, e=%d, first=%p, nfrags=%d", +- __func__, priv->cur_tx, priv->dirty_tx, first_entry, +- entry, first, nfrags); ++ netdev_dbg(priv->dev, ++ "%s: curr=%d dirty=%d f=%d, e=%d, first=%p, nfrags=%d", ++ __func__, priv->cur_tx, priv->dirty_tx, first_entry, ++ entry, first, nfrags); + + if (priv->extend_desc) + tx_head = (void *)priv->dma_etx; +@@ -2277,13 +2271,13 @@ static netdev_tx_t stmmac_xmit(struct sk + + priv->hw->desc->display_ring(tx_head, DMA_TX_SIZE, false); + +- pr_debug(">>> frame to be transmitted: "); ++ netdev_dbg(priv->dev, ">>> frame to be transmitted: "); + print_pkt(skb->data, skb->len); + } + + if (unlikely(stmmac_tx_avail(priv) <= (MAX_SKB_FRAGS + 1))) { +- if (netif_msg_hw(priv)) +- pr_debug("%s: stop transmitted packets\n", __func__); ++ netif_dbg(priv, hw, priv->dev, "%s: stop transmitted packets\n", ++ __func__); + netif_stop_queue(dev); + } + +@@ -2319,13 +2313,11 @@ static netdev_tx_t stmmac_xmit(struct sk + if (dma_mapping_error(priv->device, des)) + goto dma_map_err; + +- if (unlikely(priv->synopsys_id >= DWMAC_CORE_4_00)) { +- first->des0 = des; +- priv->tx_skbuff_dma[first_entry].buf = first->des0; +- } else { +- first->des2 = des; +- priv->tx_skbuff_dma[first_entry].buf = first->des2; +- } ++ priv->tx_skbuff_dma[first_entry].buf = des; ++ if (unlikely(priv->synopsys_id >= DWMAC_CORE_4_00)) ++ first->des0 = cpu_to_le32(des); ++ else ++ first->des2 = cpu_to_le32(des); + + priv->tx_skbuff_dma[first_entry].len = nopaged_len; + priv->tx_skbuff_dma[first_entry].last_segment = last_segment; +@@ -2346,7 +2338,7 @@ static netdev_tx_t stmmac_xmit(struct sk + * descriptor and then barrier is needed to make sure that + * all is coherent before granting the DMA engine. + */ +- smp_wmb(); ++ dma_wmb(); + } + + netdev_sent_queue(dev, skb->len); +@@ -2357,12 +2349,10 @@ static netdev_tx_t stmmac_xmit(struct sk + priv->hw->dma->set_tx_tail_ptr(priv->ioaddr, priv->tx_tail_addr, + STMMAC_CHAN0); + +- spin_unlock(&priv->tx_lock); + return NETDEV_TX_OK; + + dma_map_err: +- spin_unlock(&priv->tx_lock); +- dev_err(priv->device, "Tx dma map failed\n"); ++ netdev_err(priv->dev, "Tx DMA map failed\n"); + dev_kfree_skb(skb); + priv->dev->stats.tx_dropped++; + return NETDEV_TX_OK; +@@ -2433,16 +2423,16 @@ static inline void stmmac_rx_refill(stru + DMA_FROM_DEVICE); + if (dma_mapping_error(priv->device, + priv->rx_skbuff_dma[entry])) { +- dev_err(priv->device, "Rx dma map failed\n"); ++ netdev_err(priv->dev, "Rx DMA map failed\n"); + dev_kfree_skb(skb); + break; + } + + if (unlikely(priv->synopsys_id >= DWMAC_CORE_4_00)) { +- p->des0 = priv->rx_skbuff_dma[entry]; ++ p->des0 = cpu_to_le32(priv->rx_skbuff_dma[entry]); + p->des1 = 0; + } else { +- p->des2 = priv->rx_skbuff_dma[entry]; ++ p->des2 = cpu_to_le32(priv->rx_skbuff_dma[entry]); + } + if (priv->hw->mode->refill_desc3) + priv->hw->mode->refill_desc3(priv, p); +@@ -2450,17 +2440,17 @@ static inline void stmmac_rx_refill(stru + if (priv->rx_zeroc_thresh > 0) + priv->rx_zeroc_thresh--; + +- if (netif_msg_rx_status(priv)) +- pr_debug("\trefill entry #%d\n", entry); ++ netif_dbg(priv, rx_status, priv->dev, ++ "refill entry #%d\n", entry); + } +- wmb(); ++ dma_wmb(); + + if (unlikely(priv->synopsys_id >= DWMAC_CORE_4_00)) + priv->hw->desc->init_rx_desc(p, priv->use_riwt, 0, 0); + else + priv->hw->desc->set_rx_owner(p); + +- wmb(); ++ dma_wmb(); + + entry = STMMAC_GET_ENTRY(entry, DMA_RX_SIZE); + } +@@ -2484,7 +2474,7 @@ static int stmmac_rx(struct stmmac_priv + if (netif_msg_rx_status(priv)) { + void *rx_head; + +- pr_info(">>>>>> %s: descriptor ring:\n", __func__); ++ netdev_dbg(priv->dev, "%s: descriptor ring:\n", __func__); + if (priv->extend_desc) + rx_head = (void *)priv->dma_erx; + else +@@ -2546,9 +2536,9 @@ static int stmmac_rx(struct stmmac_priv + unsigned int des; + + if (unlikely(priv->synopsys_id >= DWMAC_CORE_4_00)) +- des = p->des0; ++ des = le32_to_cpu(p->des0); + else +- des = p->des2; ++ des = le32_to_cpu(p->des2); + + frame_len = priv->hw->desc->get_rx_frame_len(p, coe); + +@@ -2557,9 +2547,9 @@ static int stmmac_rx(struct stmmac_priv + * ignored + */ + if (frame_len > priv->dma_buf_sz) { +- pr_err("%s: len %d larger than size (%d)\n", +- priv->dev->name, frame_len, +- priv->dma_buf_sz); ++ netdev_err(priv->dev, ++ "len %d larger than size (%d)\n", ++ frame_len, priv->dma_buf_sz); + priv->dev->stats.rx_length_errors++; + break; + } +@@ -2571,11 +2561,11 @@ static int stmmac_rx(struct stmmac_priv + frame_len -= ETH_FCS_LEN; + + if (netif_msg_rx_status(priv)) { +- pr_info("\tdesc: %p [entry %d] buff=0x%x\n", +- p, entry, des); ++ netdev_dbg(priv->dev, "\tdesc: %p [entry %d] buff=0x%x\n", ++ p, entry, des); + if (frame_len > ETH_FRAME_LEN) +- pr_debug("\tframe size %d, COE: %d\n", +- frame_len, status); ++ netdev_dbg(priv->dev, "frame size %d, COE: %d\n", ++ frame_len, status); + } + + /* The zero-copy is always used for all the sizes +@@ -2612,8 +2602,9 @@ static int stmmac_rx(struct stmmac_priv + } else { + skb = priv->rx_skbuff[entry]; + if (unlikely(!skb)) { +- pr_err("%s: Inconsistent Rx chain\n", +- priv->dev->name); ++ netdev_err(priv->dev, ++ "%s: Inconsistent Rx chain\n", ++ priv->dev->name); + priv->dev->stats.rx_dropped++; + break; + } +@@ -2629,7 +2620,8 @@ static int stmmac_rx(struct stmmac_priv + } + + if (netif_msg_pktdata(priv)) { +- pr_debug("frame received (%dbytes)", frame_len); ++ netdev_dbg(priv->dev, "frame received (%dbytes)", ++ frame_len); + print_pkt(skb->data, frame_len); + } + +@@ -2732,7 +2724,7 @@ static int stmmac_change_mtu(struct net_ + int max_mtu; + + if (netif_running(dev)) { +- pr_err("%s: must be stopped to change its MTU\n", dev->name); ++ netdev_err(priv->dev, "must be stopped to change its MTU\n"); + return -EBUSY; + } + +@@ -2824,7 +2816,7 @@ static irqreturn_t stmmac_interrupt(int + pm_wakeup_event(priv->device, 0); + + if (unlikely(!dev)) { +- pr_err("%s: invalid dev pointer\n", __func__); ++ netdev_err(priv->dev, "%s: invalid dev pointer\n", __func__); + return IRQ_NONE; + } + +@@ -2882,7 +2874,6 @@ static void stmmac_poll_controller(struc + */ + static int stmmac_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) + { +- struct stmmac_priv *priv = netdev_priv(dev); + int ret = -EOPNOTSUPP; + + if (!netif_running(dev)) +@@ -2892,9 +2883,9 @@ static int stmmac_ioctl(struct net_devic + case SIOCGMIIPHY: + case SIOCGMIIREG: + case SIOCSMIIREG: +- if (!priv->phydev) ++ if (!dev->phydev) + return -EINVAL; +- ret = phy_mii_ioctl(priv->phydev, rq, cmd); ++ ret = phy_mii_ioctl(dev->phydev, rq, cmd); + break; + case SIOCSHWTSTAMP: + ret = stmmac_hwtstamp_ioctl(dev, rq); +@@ -2922,14 +2913,17 @@ static void sysfs_display_ring(void *hea + x = *(u64 *) ep; + seq_printf(seq, "%d [0x%x]: 0x%x 0x%x 0x%x 0x%x\n", + i, (unsigned int)virt_to_phys(ep), +- ep->basic.des0, ep->basic.des1, +- ep->basic.des2, ep->basic.des3); ++ le32_to_cpu(ep->basic.des0), ++ le32_to_cpu(ep->basic.des1), ++ le32_to_cpu(ep->basic.des2), ++ le32_to_cpu(ep->basic.des3)); + ep++; + } else { + x = *(u64 *) p; + seq_printf(seq, "%d [0x%x]: 0x%x 0x%x 0x%x 0x%x\n", + i, (unsigned int)virt_to_phys(ep), +- p->des0, p->des1, p->des2, p->des3); ++ le32_to_cpu(p->des0), le32_to_cpu(p->des1), ++ le32_to_cpu(p->des2), le32_to_cpu(p->des3)); + p++; + } + seq_printf(seq, "\n"); +@@ -2961,6 +2955,8 @@ static int stmmac_sysfs_ring_open(struct + return single_open(file, stmmac_sysfs_ring_read, inode->i_private); + } + ++/* Debugfs files, should appear in /sys/kernel/debug/stmmaceth/eth0 */ ++ + static const struct file_operations stmmac_rings_status_fops = { + .owner = THIS_MODULE, + .open = stmmac_sysfs_ring_open, +@@ -2983,11 +2979,11 @@ static int stmmac_sysfs_dma_cap_read(str + seq_printf(seq, "\tDMA HW features\n"); + seq_printf(seq, "==============================\n"); + +- seq_printf(seq, "\t10/100 Mbps %s\n", ++ seq_printf(seq, "\t10/100 Mbps: %s\n", + (priv->dma_cap.mbps_10_100) ? "Y" : "N"); +- seq_printf(seq, "\t1000 Mbps %s\n", ++ seq_printf(seq, "\t1000 Mbps: %s\n", + (priv->dma_cap.mbps_1000) ? "Y" : "N"); +- seq_printf(seq, "\tHalf duple %s\n", ++ seq_printf(seq, "\tHalf duplex: %s\n", + (priv->dma_cap.half_duplex) ? "Y" : "N"); + seq_printf(seq, "\tHash Filter: %s\n", + (priv->dma_cap.hash_filter) ? "Y" : "N"); +@@ -3005,9 +3001,9 @@ static int stmmac_sysfs_dma_cap_read(str + (priv->dma_cap.rmon) ? "Y" : "N"); + seq_printf(seq, "\tIEEE 1588-2002 Time Stamp: %s\n", + (priv->dma_cap.time_stamp) ? "Y" : "N"); +- seq_printf(seq, "\tIEEE 1588-2008 Advanced Time Stamp:%s\n", ++ seq_printf(seq, "\tIEEE 1588-2008 Advanced Time Stamp: %s\n", + (priv->dma_cap.atime_stamp) ? "Y" : "N"); +- seq_printf(seq, "\t802.3az - Energy-Efficient Ethernet (EEE) %s\n", ++ seq_printf(seq, "\t802.3az - Energy-Efficient Ethernet (EEE): %s\n", + (priv->dma_cap.eee) ? "Y" : "N"); + seq_printf(seq, "\tAV features: %s\n", (priv->dma_cap.av) ? "Y" : "N"); + seq_printf(seq, "\tChecksum Offload in TX: %s\n", +@@ -3054,8 +3050,7 @@ static int stmmac_init_fs(struct net_dev + priv->dbgfs_dir = debugfs_create_dir(dev->name, stmmac_fs_dir); + + if (!priv->dbgfs_dir || IS_ERR(priv->dbgfs_dir)) { +- pr_err("ERROR %s/%s, debugfs create directory failed\n", +- STMMAC_RESOURCE_NAME, dev->name); ++ netdev_err(priv->dev, "ERROR failed to create debugfs directory\n"); + + return -ENOMEM; + } +@@ -3067,7 +3062,7 @@ static int stmmac_init_fs(struct net_dev + &stmmac_rings_status_fops); + + if (!priv->dbgfs_rings_status || IS_ERR(priv->dbgfs_rings_status)) { +- pr_info("ERROR creating stmmac ring debugfs file\n"); ++ netdev_err(priv->dev, "ERROR creating stmmac ring debugfs file\n"); + debugfs_remove_recursive(priv->dbgfs_dir); + + return -ENOMEM; +@@ -3079,7 +3074,7 @@ static int stmmac_init_fs(struct net_dev + dev, &stmmac_dma_cap_fops); + + if (!priv->dbgfs_dma_cap || IS_ERR(priv->dbgfs_dma_cap)) { +- pr_info("ERROR creating stmmac MMC debugfs file\n"); ++ netdev_err(priv->dev, "ERROR creating stmmac MMC debugfs file\n"); + debugfs_remove_recursive(priv->dbgfs_dir); + + return -ENOMEM; +@@ -3151,11 +3146,11 @@ static int stmmac_hw_init(struct stmmac_ + } else { + if (chain_mode) { + priv->hw->mode = &chain_mode_ops; +- pr_info(" Chain mode enabled\n"); ++ dev_info(priv->device, "Chain mode enabled\n"); + priv->mode = STMMAC_CHAIN_MODE; + } else { + priv->hw->mode = &ring_mode_ops; +- pr_info(" Ring mode enabled\n"); ++ dev_info(priv->device, "Ring mode enabled\n"); + priv->mode = STMMAC_RING_MODE; + } + } +@@ -3163,7 +3158,7 @@ static int stmmac_hw_init(struct stmmac_ + /* Get the HW capability (new GMAC newer than 3.50a) */ + priv->hw_cap_support = stmmac_get_hw_features(priv); + if (priv->hw_cap_support) { +- pr_info(" DMA HW capability register supported"); ++ dev_info(priv->device, "DMA HW capability register supported\n"); + + /* We can override some gmac/dma configuration fields: e.g. + * enh_desc, tx_coe (e.g. that are passed through the +@@ -3188,8 +3183,9 @@ static int stmmac_hw_init(struct stmmac_ + else if (priv->dma_cap.rx_coe_type1) + priv->plat->rx_coe = STMMAC_RX_COE_TYPE1; + +- } else +- pr_info(" No HW DMA feature register supported"); ++ } else { ++ dev_info(priv->device, "No HW DMA feature register supported\n"); ++ } + + /* To use alternate (extended), normal or GMAC4 descriptor structures */ + if (priv->synopsys_id >= DWMAC_CORE_4_00) +@@ -3199,20 +3195,20 @@ static int stmmac_hw_init(struct stmmac_ + + if (priv->plat->rx_coe) { + priv->hw->rx_csum = priv->plat->rx_coe; +- pr_info(" RX Checksum Offload Engine supported\n"); ++ dev_info(priv->device, "RX Checksum Offload Engine supported\n"); + if (priv->synopsys_id < DWMAC_CORE_4_00) +- pr_info("\tCOE Type %d\n", priv->hw->rx_csum); ++ dev_info(priv->device, "COE Type %d\n", priv->hw->rx_csum); + } + if (priv->plat->tx_coe) +- pr_info(" TX Checksum insertion supported\n"); ++ dev_info(priv->device, "TX Checksum insertion supported\n"); + + if (priv->plat->pmt) { +- pr_info(" Wake-Up On Lan supported\n"); ++ dev_info(priv->device, "Wake-Up On Lan supported\n"); + device_set_wakeup_capable(priv->device, 1); + } + + if (priv->dma_cap.tsoen) +- pr_info(" TSO supported\n"); ++ dev_info(priv->device, "TSO supported\n"); + + return 0; + } +@@ -3271,8 +3267,8 @@ int stmmac_dvr_probe(struct device *devi + + priv->stmmac_clk = devm_clk_get(priv->device, STMMAC_RESOURCE_NAME); + if (IS_ERR(priv->stmmac_clk)) { +- dev_warn(priv->device, "%s: warning: cannot get CSR clock\n", +- __func__); ++ netdev_warn(priv->dev, "%s: warning: cannot get CSR clock\n", ++ __func__); + /* If failed to obtain stmmac_clk and specific clk_csr value + * is NOT passed from the platform, probe fail. + */ +@@ -3321,7 +3317,7 @@ int stmmac_dvr_probe(struct device *devi + if ((priv->plat->tso_en) && (priv->dma_cap.tsoen)) { + ndev->hw_features |= NETIF_F_TSO; + priv->tso = true; +- pr_info(" TSO feature enabled\n"); ++ dev_info(priv->device, "TSO feature enabled\n"); + } + ndev->features |= ndev->hw_features | NETIF_F_HIGHDMA; + ndev->watchdog_timeo = msecs_to_jiffies(watchdog); +@@ -3341,13 +3337,13 @@ int stmmac_dvr_probe(struct device *devi + */ + if ((priv->synopsys_id >= DWMAC_CORE_3_50) && (!priv->plat->riwt_off)) { + priv->use_riwt = 1; +- pr_info(" Enable RX Mitigation via HW Watchdog Timer\n"); ++ dev_info(priv->device, ++ "Enable RX Mitigation via HW Watchdog Timer\n"); + } + + netif_napi_add(ndev, &priv->napi, stmmac_poll, 64); + + spin_lock_init(&priv->lock); +- spin_lock_init(&priv->tx_lock); + + /* If a specific clk_csr value is passed from the platform + * this means that the CSR Clock Range selection cannot be +@@ -3368,15 +3364,17 @@ int stmmac_dvr_probe(struct device *devi + /* MDIO bus Registration */ + ret = stmmac_mdio_register(ndev); + if (ret < 0) { +- pr_debug("%s: MDIO bus (id: %d) registration failed", +- __func__, priv->plat->bus_id); +- goto error_napi_register; ++ dev_err(priv->device, ++ "%s: MDIO bus (id: %d) registration failed", ++ __func__, priv->plat->bus_id); ++ goto error_mdio_register; + } + } + + ret = register_netdev(ndev); + if (ret) { +- pr_err("%s: ERROR %i registering the device\n", __func__, ret); ++ dev_err(priv->device, "%s: ERROR %i registering the device\n", ++ __func__, ret); + goto error_netdev_register; + } + +@@ -3387,7 +3385,7 @@ error_netdev_register: + priv->hw->pcs != STMMAC_PCS_TBI && + priv->hw->pcs != STMMAC_PCS_RTBI) + stmmac_mdio_unregister(ndev); +-error_napi_register: ++error_mdio_register: + netif_napi_del(&priv->napi); + error_hw_init: + clk_disable_unprepare(priv->pclk); +@@ -3411,7 +3409,7 @@ int stmmac_dvr_remove(struct device *dev + struct net_device *ndev = dev_get_drvdata(dev); + struct stmmac_priv *priv = netdev_priv(ndev); + +- pr_info("%s:\n\tremoving driver", __func__); ++ netdev_info(priv->dev, "%s: removing driver", __func__); + + priv->hw->dma->stop_rx(priv->ioaddr); + priv->hw->dma->stop_tx(priv->ioaddr); +@@ -3449,8 +3447,8 @@ int stmmac_suspend(struct device *dev) + if (!ndev || !netif_running(ndev)) + return 0; + +- if (priv->phydev) +- phy_stop(priv->phydev); ++ if (ndev->phydev) ++ phy_stop(ndev->phydev); + + spin_lock_irqsave(&priv->lock, flags); + +@@ -3544,8 +3542,8 @@ int stmmac_resume(struct device *dev) + + spin_unlock_irqrestore(&priv->lock, flags); + +- if (priv->phydev) +- phy_start(priv->phydev); ++ if (ndev->phydev) ++ phy_start(ndev->phydev); + + return 0; + } +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c +@@ -42,13 +42,6 @@ + #define MII_GMAC4_WRITE (1 << MII_GMAC4_GOC_SHIFT) + #define MII_GMAC4_READ (3 << MII_GMAC4_GOC_SHIFT) + +-#define MII_PHY_ADDR_GMAC4_SHIFT 21 +-#define MII_PHY_ADDR_GMAC4_MASK GENMASK(25, 21) +-#define MII_PHY_REG_GMAC4_SHIFT 16 +-#define MII_PHY_REG_GMAC4_MASK GENMASK(20, 16) +-#define MII_CSR_CLK_GMAC4_SHIFT 8 +-#define MII_CSR_CLK_GMAC4_MASK GENMASK(11, 8) +- + static int stmmac_mdio_busy_wait(void __iomem *ioaddr, unsigned int mii_addr) + { + unsigned long curr; +@@ -68,8 +61,8 @@ static int stmmac_mdio_busy_wait(void __ + /** + * stmmac_mdio_read + * @bus: points to the mii_bus structure +- * @phyaddr: MII addr reg bits 15-11 +- * @phyreg: MII addr reg bits 10-6 ++ * @phyaddr: MII addr ++ * @phyreg: MII reg + * Description: it reads data from the MII register from within the phy device. + * For the 7111 GMAC, we must set the bit 0 in the MII address register while + * accessing the PHY registers. +@@ -83,14 +76,20 @@ static int stmmac_mdio_read(struct mii_b + unsigned int mii_data = priv->hw->mii.data; + + int data; +- u16 regValue = (((phyaddr << 11) & (0x0000F800)) | +- ((phyreg << 6) & (0x000007C0))); +- regValue |= MII_BUSY | ((priv->clk_csr & 0xF) << 2); ++ u32 value = MII_BUSY; ++ ++ value |= (phyaddr << priv->hw->mii.addr_shift) ++ & priv->hw->mii.addr_mask; ++ value |= (phyreg << priv->hw->mii.reg_shift) & priv->hw->mii.reg_mask; ++ value |= (priv->clk_csr << priv->hw->mii.clk_csr_shift) ++ & priv->hw->mii.clk_csr_mask; ++ if (priv->plat->has_gmac4) ++ value |= MII_GMAC4_READ; + + if (stmmac_mdio_busy_wait(priv->ioaddr, mii_address)) + return -EBUSY; + +- writel(regValue, priv->ioaddr + mii_address); ++ writel(value, priv->ioaddr + mii_address); + + if (stmmac_mdio_busy_wait(priv->ioaddr, mii_address)) + return -EBUSY; +@@ -104,8 +103,8 @@ static int stmmac_mdio_read(struct mii_b + /** + * stmmac_mdio_write + * @bus: points to the mii_bus structure +- * @phyaddr: MII addr reg bits 15-11 +- * @phyreg: MII addr reg bits 10-6 ++ * @phyaddr: MII addr ++ * @phyreg: MII reg + * @phydata: phy data + * Description: it writes the data into the MII register from within the device. + */ +@@ -117,85 +116,18 @@ static int stmmac_mdio_write(struct mii_ + unsigned int mii_address = priv->hw->mii.addr; + unsigned int mii_data = priv->hw->mii.data; + +- u16 value = +- (((phyaddr << 11) & (0x0000F800)) | ((phyreg << 6) & (0x000007C0))) +- | MII_WRITE; +- +- value |= MII_BUSY | ((priv->clk_csr & 0xF) << 2); +- +- /* Wait until any existing MII operation is complete */ +- if (stmmac_mdio_busy_wait(priv->ioaddr, mii_address)) +- return -EBUSY; +- +- /* Set the MII address register to write */ +- writel(phydata, priv->ioaddr + mii_data); +- writel(value, priv->ioaddr + mii_address); +- +- /* Wait until any existing MII operation is complete */ +- return stmmac_mdio_busy_wait(priv->ioaddr, mii_address); +-} +- +-/** +- * stmmac_mdio_read_gmac4 +- * @bus: points to the mii_bus structure +- * @phyaddr: MII addr reg bits 25-21 +- * @phyreg: MII addr reg bits 20-16 +- * Description: it reads data from the MII register of GMAC4 from within +- * the phy device. +- */ +-static int stmmac_mdio_read_gmac4(struct mii_bus *bus, int phyaddr, int phyreg) +-{ +- struct net_device *ndev = bus->priv; +- struct stmmac_priv *priv = netdev_priv(ndev); +- unsigned int mii_address = priv->hw->mii.addr; +- unsigned int mii_data = priv->hw->mii.data; +- int data; +- u32 value = (((phyaddr << MII_PHY_ADDR_GMAC4_SHIFT) & +- (MII_PHY_ADDR_GMAC4_MASK)) | +- ((phyreg << MII_PHY_REG_GMAC4_SHIFT) & +- (MII_PHY_REG_GMAC4_MASK))) | MII_GMAC4_READ; +- +- value |= MII_BUSY | ((priv->clk_csr & MII_CSR_CLK_GMAC4_MASK) +- << MII_CSR_CLK_GMAC4_SHIFT); +- +- if (stmmac_mdio_busy_wait(priv->ioaddr, mii_address)) +- return -EBUSY; +- +- writel(value, priv->ioaddr + mii_address); +- +- if (stmmac_mdio_busy_wait(priv->ioaddr, mii_address)) +- return -EBUSY; +- +- /* Read the data from the MII data register */ +- data = (int)readl(priv->ioaddr + mii_data); +- +- return data; +-} +- +-/** +- * stmmac_mdio_write_gmac4 +- * @bus: points to the mii_bus structure +- * @phyaddr: MII addr reg bits 25-21 +- * @phyreg: MII addr reg bits 20-16 +- * @phydata: phy data +- * Description: it writes the data into the MII register of GMAC4 from within +- * the device. +- */ +-static int stmmac_mdio_write_gmac4(struct mii_bus *bus, int phyaddr, int phyreg, +- u16 phydata) +-{ +- struct net_device *ndev = bus->priv; +- struct stmmac_priv *priv = netdev_priv(ndev); +- unsigned int mii_address = priv->hw->mii.addr; +- unsigned int mii_data = priv->hw->mii.data; +- +- u32 value = (((phyaddr << MII_PHY_ADDR_GMAC4_SHIFT) & +- (MII_PHY_ADDR_GMAC4_MASK)) | +- ((phyreg << MII_PHY_REG_GMAC4_SHIFT) & +- (MII_PHY_REG_GMAC4_MASK))) | MII_GMAC4_WRITE; ++ u32 value = MII_BUSY; + +- value |= MII_BUSY | ((priv->clk_csr & MII_CSR_CLK_GMAC4_MASK) +- << MII_CSR_CLK_GMAC4_SHIFT); ++ value |= (phyaddr << priv->hw->mii.addr_shift) ++ & priv->hw->mii.addr_mask; ++ value |= (phyreg << priv->hw->mii.reg_shift) & priv->hw->mii.reg_mask; ++ ++ value |= (priv->clk_csr << priv->hw->mii.clk_csr_shift) ++ & priv->hw->mii.clk_csr_mask; ++ if (priv->plat->has_gmac4) ++ value |= MII_GMAC4_WRITE; ++ else ++ value |= MII_WRITE; + + /* Wait until any existing MII operation is complete */ + if (stmmac_mdio_busy_wait(priv->ioaddr, mii_address)) +@@ -260,7 +192,7 @@ int stmmac_mdio_reset(struct mii_bus *bu + #endif + + if (data->phy_reset) { +- pr_debug("stmmac_mdio_reset: calling phy_reset\n"); ++ netdev_dbg(ndev, "stmmac_mdio_reset: calling phy_reset\n"); + data->phy_reset(priv->plat->bsp_priv); + } + +@@ -305,13 +237,8 @@ int stmmac_mdio_register(struct net_devi + #endif + + new_bus->name = "stmmac"; +- if (priv->plat->has_gmac4) { +- new_bus->read = &stmmac_mdio_read_gmac4; +- new_bus->write = &stmmac_mdio_write_gmac4; +- } else { +- new_bus->read = &stmmac_mdio_read; +- new_bus->write = &stmmac_mdio_write; +- } ++ new_bus->read = &stmmac_mdio_read; ++ new_bus->write = &stmmac_mdio_write; + + new_bus->reset = &stmmac_mdio_reset; + snprintf(new_bus->id, MII_BUS_ID_SIZE, "%s-%x", +@@ -325,7 +252,7 @@ int stmmac_mdio_register(struct net_devi + else + err = mdiobus_register(new_bus); + if (err != 0) { +- pr_err("%s: Cannot register as MDIO bus\n", new_bus->name); ++ netdev_err(ndev, "Cannot register the MDIO bus\n"); + goto bus_register_fail; + } + +@@ -372,16 +299,16 @@ int stmmac_mdio_register(struct net_devi + irq_str = irq_num; + break; + } +- pr_info("%s: PHY ID %08x at %d IRQ %s (%s)%s\n", +- ndev->name, phydev->phy_id, addr, +- irq_str, phydev_name(phydev), +- act ? " active" : ""); ++ netdev_info(ndev, "PHY ID %08x at %d IRQ %s (%s)%s\n", ++ phydev->phy_id, addr, ++ irq_str, phydev_name(phydev), ++ act ? " active" : ""); + found = 1; + } + } + + if (!found && !mdio_node) { +- pr_warn("%s: No PHY found\n", ndev->name); ++ netdev_warn(ndev, "No PHY found\n"); + mdiobus_unregister(new_bus); + mdiobus_free(new_bus); + return -ENODEV; +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c +@@ -81,6 +81,7 @@ static void stmmac_default_data(struct p + plat->mdio_bus_data->phy_mask = 0; + + plat->dma_cfg->pbl = 32; ++ plat->dma_cfg->pblx8 = true; + /* TODO: AXI */ + + /* Set default value for multicast hash bins */ +@@ -88,6 +89,9 @@ static void stmmac_default_data(struct p + + /* Set default value for unicast filter entries */ + plat->unicast_filter_entries = 1; ++ ++ /* Set the maxmtu to a default of JUMBO_LEN */ ++ plat->maxmtu = JUMBO_LEN; + } + + static int quark_default_data(struct plat_stmmacenet_data *plat, +@@ -115,6 +119,7 @@ static int quark_default_data(struct pla + plat->mdio_bus_data->phy_mask = 0; + + plat->dma_cfg->pbl = 16; ++ plat->dma_cfg->pblx8 = true; + plat->dma_cfg->fixed_burst = 1; + /* AXI (TODO) */ + +@@ -124,6 +129,9 @@ static int quark_default_data(struct pla + /* Set default value for unicast filter entries */ + plat->unicast_filter_entries = 1; + ++ /* Set the maxmtu to a default of JUMBO_LEN */ ++ plat->maxmtu = JUMBO_LEN; ++ + return 0; + } + +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c +@@ -292,6 +292,7 @@ stmmac_probe_config_dt(struct platform_d + if (of_device_is_compatible(np, "snps,dwmac-4.00") || + of_device_is_compatible(np, "snps,dwmac-4.10a")) { + plat->has_gmac4 = 1; ++ plat->has_gmac = 0; + plat->pmt = 1; + plat->tso_en = of_property_read_bool(np, "snps,tso"); + } +@@ -303,21 +304,25 @@ stmmac_probe_config_dt(struct platform_d + plat->force_sf_dma_mode = 1; + } + +- if (of_find_property(np, "snps,pbl", NULL)) { +- dma_cfg = devm_kzalloc(&pdev->dev, sizeof(*dma_cfg), +- GFP_KERNEL); +- if (!dma_cfg) { +- stmmac_remove_config_dt(pdev, plat); +- return ERR_PTR(-ENOMEM); +- } +- plat->dma_cfg = dma_cfg; +- of_property_read_u32(np, "snps,pbl", &dma_cfg->pbl); +- dma_cfg->aal = of_property_read_bool(np, "snps,aal"); +- dma_cfg->fixed_burst = +- of_property_read_bool(np, "snps,fixed-burst"); +- dma_cfg->mixed_burst = +- of_property_read_bool(np, "snps,mixed-burst"); +- } ++ dma_cfg = devm_kzalloc(&pdev->dev, sizeof(*dma_cfg), ++ GFP_KERNEL); ++ if (!dma_cfg) { ++ stmmac_remove_config_dt(pdev, plat); ++ return ERR_PTR(-ENOMEM); ++ } ++ plat->dma_cfg = dma_cfg; ++ ++ of_property_read_u32(np, "snps,pbl", &dma_cfg->pbl); ++ if (!dma_cfg->pbl) ++ dma_cfg->pbl = DEFAULT_DMA_PBL; ++ of_property_read_u32(np, "snps,txpbl", &dma_cfg->txpbl); ++ of_property_read_u32(np, "snps,rxpbl", &dma_cfg->rxpbl); ++ dma_cfg->pblx8 = !of_property_read_bool(np, "snps,no-pbl-x8"); ++ ++ dma_cfg->aal = of_property_read_bool(np, "snps,aal"); ++ dma_cfg->fixed_burst = of_property_read_bool(np, "snps,fixed-burst"); ++ dma_cfg->mixed_burst = of_property_read_bool(np, "snps,mixed-burst"); ++ + plat->force_thresh_dma_mode = of_property_read_bool(np, "snps,force_thresh_dma_mode"); + if (plat->force_thresh_dma_mode) { + plat->force_sf_dma_mode = 0; +@@ -445,9 +450,7 @@ static int stmmac_pltfr_suspend(struct d + struct platform_device *pdev = to_platform_device(dev); + + ret = stmmac_suspend(dev); +- if (priv->plat->suspend) +- priv->plat->suspend(pdev, priv->plat->bsp_priv); +- else if (priv->plat->exit) ++ if (priv->plat->exit) + priv->plat->exit(pdev, priv->plat->bsp_priv); + + return ret; +@@ -466,9 +469,7 @@ static int stmmac_pltfr_resume(struct de + struct stmmac_priv *priv = netdev_priv(ndev); + struct platform_device *pdev = to_platform_device(dev); + +- if (priv->plat->resume) +- priv->plat->resume(pdev, priv->plat->bsp_priv); +- else if (priv->plat->init) ++ if (priv->plat->init) + priv->plat->init(pdev, priv->plat->bsp_priv); + + return stmmac_resume(dev); +--- a/include/linux/stmmac.h ++++ b/include/linux/stmmac.h +@@ -88,6 +88,9 @@ struct stmmac_mdio_bus_data { + + struct stmmac_dma_cfg { + int pbl; ++ int txpbl; ++ int rxpbl; ++ bool pblx8; + int fixed_burst; + int mixed_burst; + bool aal; +@@ -135,8 +138,6 @@ struct plat_stmmacenet_data { + void (*bus_setup)(void __iomem *ioaddr); + int (*init)(struct platform_device *pdev, void *priv); + void (*exit)(struct platform_device *pdev, void *priv); +- void (*suspend)(struct platform_device *pdev, void *priv); +- void (*resume)(struct platform_device *pdev, void *priv); + void *bsp_priv; + struct stmmac_axi *axi; + int has_gmac4; diff --git a/target/linux/sunxi/patches-4.9/0051-stmmac-form-4-11.patch b/target/linux/sunxi/patches-4.9/0051-stmmac-form-4-11.patch new file mode 100644 index 0000000000..f3b4262307 --- /dev/null +++ b/target/linux/sunxi/patches-4.9/0051-stmmac-form-4-11.patch @@ -0,0 +1,2305 @@ +--- a/Documentation/devicetree/bindings/net/stmmac.txt ++++ b/Documentation/devicetree/bindings/net/stmmac.txt +@@ -49,6 +49,8 @@ Optional properties: + - snps,force_sf_dma_mode Force DMA to use the Store and Forward + mode for both tx and rx. This flag is + ignored if force_thresh_dma_mode is set. ++- snps,en-tx-lpi-clockgating Enable gating of the MAC TX clock during ++ TX low-power mode + - snps,multicast-filter-bins: Number of multicast filter hash bins + supported by this device instance + - snps,perfect-filter-entries: Number of perfect filter entries supported +@@ -65,7 +67,6 @@ Optional properties: + - snps,wr_osr_lmt: max write outstanding req. limit + - snps,rd_osr_lmt: max read outstanding req. limit + - snps,kbbe: do not cross 1KiB boundary. +- - snps,axi_all: align address + - snps,blen: this is a vector of supported burst length. + - snps,fb: fixed-burst + - snps,mb: mixed-burst +--- a/drivers/net/ethernet/stmicro/stmmac/Kconfig ++++ b/drivers/net/ethernet/stmicro/stmmac/Kconfig +@@ -1,5 +1,5 @@ + config STMMAC_ETH +- tristate "STMicroelectronics 10/100/1000 Ethernet driver" ++ tristate "STMicroelectronics 10/100/1000/EQOS Ethernet driver" + depends on HAS_IOMEM && HAS_DMA + select MII + select PHYLIB +@@ -7,9 +7,8 @@ config STMMAC_ETH + select PTP_1588_CLOCK + select RESET_CONTROLLER + ---help--- +- This is the driver for the Ethernet IPs are built around a +- Synopsys IP Core and only tested on the STMicroelectronics +- platforms. ++ This is the driver for the Ethernet IPs built around a ++ Synopsys IP Core. + + if STMMAC_ETH + +@@ -29,6 +28,15 @@ config STMMAC_PLATFORM + + if STMMAC_PLATFORM + ++config DWMAC_DWC_QOS_ETH ++ tristate "Support for snps,dwc-qos-ethernet.txt DT binding." ++ select PHYLIB ++ select CRC32 ++ select MII ++ depends on OF && HAS_DMA ++ help ++ Support for chips using the snps,dwc-qos-ethernet.txt DT binding. ++ + config DWMAC_GENERIC + tristate "Generic driver for DWMAC" + default STMMAC_PLATFORM +@@ -143,11 +151,11 @@ config STMMAC_PCI + tristate "STMMAC PCI bus support" + depends on STMMAC_ETH && PCI + ---help--- +- This is to select the Synopsys DWMAC available on PCI devices, +- if you have a controller with this interface, say Y or M here. ++ This selects the platform specific bus support for the stmmac driver. ++ This driver was tested on XLINX XC2V3000 FF1152AMT0221 ++ D1215994A VIRTEX FPGA board and SNPS QoS IPK Prototyping Kit. + +- This PCI support is tested on XLINX XC2V3000 FF1152AMT0221 +- D1215994A VIRTEX FPGA board. ++ If you have a controller with this interface, say Y or M here. + + If unsure, say N. + endif +--- a/drivers/net/ethernet/stmicro/stmmac/Makefile ++++ b/drivers/net/ethernet/stmicro/stmmac/Makefile +@@ -16,6 +16,7 @@ obj-$(CONFIG_DWMAC_SOCFPGA) += dwmac-alt + obj-$(CONFIG_DWMAC_STI) += dwmac-sti.o + obj-$(CONFIG_DWMAC_STM32) += dwmac-stm32.o + obj-$(CONFIG_DWMAC_SUNXI) += dwmac-sunxi.o ++obj-$(CONFIG_DWMAC_DWC_QOS_ETH) += dwmac-dwc-qos-eth.o + obj-$(CONFIG_DWMAC_GENERIC) += dwmac-generic.o + stmmac-platform-objs:= stmmac_platform.o + dwmac-altr-socfpga-objs := altr_tse_pcs.o dwmac-socfpga.o +--- a/drivers/net/ethernet/stmicro/stmmac/chain_mode.c ++++ b/drivers/net/ethernet/stmicro/stmmac/chain_mode.c +@@ -16,10 +16,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- a/drivers/net/ethernet/stmicro/stmmac/common.h ++++ b/drivers/net/ethernet/stmicro/stmmac/common.h +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -71,7 +67,7 @@ struct stmmac_extra_stats { + unsigned long overflow_error; + unsigned long ipc_csum_error; + unsigned long rx_collision; +- unsigned long rx_crc; ++ unsigned long rx_crc_errors; + unsigned long dribbling_bit; + unsigned long rx_length; + unsigned long rx_mii; +@@ -323,6 +319,9 @@ struct dma_features { + /* TX and RX number of channels */ + unsigned int number_rx_channel; + unsigned int number_tx_channel; ++ /* TX and RX number of queues */ ++ unsigned int number_rx_queues; ++ unsigned int number_tx_queues; + /* Alternate (enhanced) DESC mode */ + unsigned int enh_desc; + }; +@@ -340,7 +339,7 @@ struct dma_features { + /* Common MAC defines */ + #define MAC_CTRL_REG 0x00000000 /* MAC Control */ + #define MAC_ENABLE_TX 0x00000008 /* Transmitter Enable */ +-#define MAC_RNABLE_RX 0x00000004 /* Receiver Enable */ ++#define MAC_ENABLE_RX 0x00000004 /* Receiver Enable */ + + /* Default LPI timers */ + #define STMMAC_DEFAULT_LIT_LS 0x3E8 +@@ -417,7 +416,7 @@ struct stmmac_dma_ops { + /* Configure the AXI Bus Mode Register */ + void (*axi)(void __iomem *ioaddr, struct stmmac_axi *axi); + /* Dump DMA registers */ +- void (*dump_regs) (void __iomem *ioaddr); ++ void (*dump_regs)(void __iomem *ioaddr, u32 *reg_space); + /* Set tx/rx threshold in the csr6 register + * An invalid value enables the store-and-forward mode */ + void (*dma_mode)(void __iomem *ioaddr, int txmode, int rxmode, +@@ -454,8 +453,10 @@ struct stmmac_ops { + void (*core_init)(struct mac_device_info *hw, int mtu); + /* Enable and verify that the IPC module is supported */ + int (*rx_ipc)(struct mac_device_info *hw); ++ /* Enable RX Queues */ ++ void (*rx_queue_enable)(struct mac_device_info *hw, u32 queue); + /* Dump MAC registers */ +- void (*dump_regs)(struct mac_device_info *hw); ++ void (*dump_regs)(struct mac_device_info *hw, u32 *reg_space); + /* Handle extra events on specific interrupts hw dependent */ + int (*host_irq_status)(struct mac_device_info *hw, + struct stmmac_extra_stats *x); +@@ -471,7 +472,8 @@ struct stmmac_ops { + unsigned int reg_n); + void (*get_umac_addr)(struct mac_device_info *hw, unsigned char *addr, + unsigned int reg_n); +- void (*set_eee_mode)(struct mac_device_info *hw); ++ void (*set_eee_mode)(struct mac_device_info *hw, ++ bool en_tx_lpi_clockgating); + void (*reset_eee_mode)(struct mac_device_info *hw); + void (*set_eee_timer)(struct mac_device_info *hw, int ls, int tw); + void (*set_eee_pls)(struct mac_device_info *hw, int link); +--- a/drivers/net/ethernet/stmicro/stmmac/descs.h ++++ b/drivers/net/ethernet/stmicro/stmmac/descs.h +@@ -11,10 +11,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- a/drivers/net/ethernet/stmicro/stmmac/descs_com.h ++++ b/drivers/net/ethernet/stmicro/stmmac/descs_com.h +@@ -17,10 +17,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- /dev/null ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-dwc-qos-eth.c +@@ -0,0 +1,202 @@ ++/* ++ * Synopsys DWC Ethernet Quality-of-Service v4.10a linux driver ++ * ++ * Copyright (C) 2016 Joao Pinto ++ * ++ * This program is free software; you can redistribute it and/or modify ++ * it under the terms of the GNU General Public License version 2 as ++ * published by the Free Software Foundation. ++ * ++ * You should have received a copy of the GNU General Public License ++ * along with this program. If not, see . ++ */ ++ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#include "stmmac_platform.h" ++ ++static int dwc_eth_dwmac_config_dt(struct platform_device *pdev, ++ struct plat_stmmacenet_data *plat_dat) ++{ ++ struct device_node *np = pdev->dev.of_node; ++ u32 burst_map = 0; ++ u32 bit_index = 0; ++ u32 a_index = 0; ++ ++ if (!plat_dat->axi) { ++ plat_dat->axi = kzalloc(sizeof(struct stmmac_axi), GFP_KERNEL); ++ ++ if (!plat_dat->axi) ++ return -ENOMEM; ++ } ++ ++ plat_dat->axi->axi_lpi_en = of_property_read_bool(np, "snps,en-lpi"); ++ if (of_property_read_u32(np, "snps,write-requests", ++ &plat_dat->axi->axi_wr_osr_lmt)) { ++ /** ++ * Since the register has a reset value of 1, if property ++ * is missing, default to 1. ++ */ ++ plat_dat->axi->axi_wr_osr_lmt = 1; ++ } else { ++ /** ++ * If property exists, to keep the behavior from dwc_eth_qos, ++ * subtract one after parsing. ++ */ ++ plat_dat->axi->axi_wr_osr_lmt--; ++ } ++ ++ if (of_property_read_u32(np, "read,read-requests", ++ &plat_dat->axi->axi_rd_osr_lmt)) { ++ /** ++ * Since the register has a reset value of 1, if property ++ * is missing, default to 1. ++ */ ++ plat_dat->axi->axi_rd_osr_lmt = 1; ++ } else { ++ /** ++ * If property exists, to keep the behavior from dwc_eth_qos, ++ * subtract one after parsing. ++ */ ++ plat_dat->axi->axi_rd_osr_lmt--; ++ } ++ of_property_read_u32(np, "snps,burst-map", &burst_map); ++ ++ /* converts burst-map bitmask to burst array */ ++ for (bit_index = 0; bit_index < 7; bit_index++) { ++ if (burst_map & (1 << bit_index)) { ++ switch (bit_index) { ++ case 0: ++ plat_dat->axi->axi_blen[a_index] = 4; break; ++ case 1: ++ plat_dat->axi->axi_blen[a_index] = 8; break; ++ case 2: ++ plat_dat->axi->axi_blen[a_index] = 16; break; ++ case 3: ++ plat_dat->axi->axi_blen[a_index] = 32; break; ++ case 4: ++ plat_dat->axi->axi_blen[a_index] = 64; break; ++ case 5: ++ plat_dat->axi->axi_blen[a_index] = 128; break; ++ case 6: ++ plat_dat->axi->axi_blen[a_index] = 256; break; ++ default: ++ break; ++ } ++ a_index++; ++ } ++ } ++ ++ /* dwc-qos needs GMAC4, AAL, TSO and PMT */ ++ plat_dat->has_gmac4 = 1; ++ plat_dat->dma_cfg->aal = 1; ++ plat_dat->tso_en = 1; ++ plat_dat->pmt = 1; ++ ++ return 0; ++} ++ ++static int dwc_eth_dwmac_probe(struct platform_device *pdev) ++{ ++ struct plat_stmmacenet_data *plat_dat; ++ struct stmmac_resources stmmac_res; ++ struct resource *res; ++ int ret; ++ ++ memset(&stmmac_res, 0, sizeof(struct stmmac_resources)); ++ ++ /** ++ * Since stmmac_platform supports name IRQ only, basic platform ++ * resource initialization is done in the glue logic. ++ */ ++ stmmac_res.irq = platform_get_irq(pdev, 0); ++ if (stmmac_res.irq < 0) { ++ if (stmmac_res.irq != -EPROBE_DEFER) ++ dev_err(&pdev->dev, ++ "IRQ configuration information not found\n"); ++ ++ return stmmac_res.irq; ++ } ++ stmmac_res.wol_irq = stmmac_res.irq; ++ ++ res = platform_get_resource(pdev, IORESOURCE_MEM, 0); ++ stmmac_res.addr = devm_ioremap_resource(&pdev->dev, res); ++ if (IS_ERR(stmmac_res.addr)) ++ return PTR_ERR(stmmac_res.addr); ++ ++ plat_dat = stmmac_probe_config_dt(pdev, &stmmac_res.mac); ++ if (IS_ERR(plat_dat)) ++ return PTR_ERR(plat_dat); ++ ++ plat_dat->stmmac_clk = devm_clk_get(&pdev->dev, "apb_pclk"); ++ if (IS_ERR(plat_dat->stmmac_clk)) { ++ dev_err(&pdev->dev, "apb_pclk clock not found.\n"); ++ ret = PTR_ERR(plat_dat->stmmac_clk); ++ plat_dat->stmmac_clk = NULL; ++ goto err_remove_config_dt; ++ } ++ clk_prepare_enable(plat_dat->stmmac_clk); ++ ++ plat_dat->pclk = devm_clk_get(&pdev->dev, "phy_ref_clk"); ++ if (IS_ERR(plat_dat->pclk)) { ++ dev_err(&pdev->dev, "phy_ref_clk clock not found.\n"); ++ ret = PTR_ERR(plat_dat->pclk); ++ plat_dat->pclk = NULL; ++ goto err_out_clk_dis_phy; ++ } ++ clk_prepare_enable(plat_dat->pclk); ++ ++ ret = dwc_eth_dwmac_config_dt(pdev, plat_dat); ++ if (ret) ++ goto err_out_clk_dis_aper; ++ ++ ret = stmmac_dvr_probe(&pdev->dev, plat_dat, &stmmac_res); ++ if (ret) ++ goto err_out_clk_dis_aper; ++ ++ return 0; ++ ++err_out_clk_dis_aper: ++ clk_disable_unprepare(plat_dat->pclk); ++err_out_clk_dis_phy: ++ clk_disable_unprepare(plat_dat->stmmac_clk); ++err_remove_config_dt: ++ stmmac_remove_config_dt(pdev, plat_dat); ++ ++ return ret; ++} ++ ++static int dwc_eth_dwmac_remove(struct platform_device *pdev) ++{ ++ return stmmac_pltfr_remove(pdev); ++} ++ ++static const struct of_device_id dwc_eth_dwmac_match[] = { ++ { .compatible = "snps,dwc-qos-ethernet-4.10", }, ++ { } ++}; ++MODULE_DEVICE_TABLE(of, dwc_eth_dwmac_match); ++ ++static struct platform_driver dwc_eth_dwmac_driver = { ++ .probe = dwc_eth_dwmac_probe, ++ .remove = dwc_eth_dwmac_remove, ++ .driver = { ++ .name = "dwc-eth-dwmac", ++ .of_match_table = dwc_eth_dwmac_match, ++ }, ++}; ++module_platform_driver(dwc_eth_dwmac_driver); ++ ++MODULE_AUTHOR("Joao Pinto "); ++MODULE_DESCRIPTION("Synopsys DWC Ethernet Quality-of-Service v4.10a driver"); ++MODULE_LICENSE("GPL v2"); +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-meson8b.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-meson8b.c +@@ -35,10 +35,6 @@ + + #define PRG_ETH0_TXDLY_SHIFT 5 + #define PRG_ETH0_TXDLY_MASK GENMASK(6, 5) +-#define PRG_ETH0_TXDLY_OFF (0x0 << PRG_ETH0_TXDLY_SHIFT) +-#define PRG_ETH0_TXDLY_QUARTER (0x1 << PRG_ETH0_TXDLY_SHIFT) +-#define PRG_ETH0_TXDLY_HALF (0x2 << PRG_ETH0_TXDLY_SHIFT) +-#define PRG_ETH0_TXDLY_THREE_QUARTERS (0x3 << PRG_ETH0_TXDLY_SHIFT) + + /* divider for the result of m250_sel */ + #define PRG_ETH0_CLK_M250_DIV_SHIFT 7 +@@ -69,6 +65,8 @@ struct meson8b_dwmac { + + struct clk_divider m25_div; + struct clk *m25_div_clk; ++ ++ u32 tx_delay_ns; + }; + + static void meson8b_dwmac_mask_bits(struct meson8b_dwmac *dwmac, u32 reg, +@@ -179,11 +177,19 @@ static int meson8b_init_prg_eth(struct m + { + int ret; + unsigned long clk_rate; ++ u8 tx_dly_val = 0; + + switch (dwmac->phy_mode) { + case PHY_INTERFACE_MODE_RGMII: +- case PHY_INTERFACE_MODE_RGMII_ID: + case PHY_INTERFACE_MODE_RGMII_RXID: ++ /* TX clock delay in ns = "8ns / 4 * tx_dly_val" (where ++ * 8ns are exactly one cycle of the 125MHz RGMII TX clock): ++ * 0ns = 0x0, 2ns = 0x1, 4ns = 0x2, 6ns = 0x3 ++ */ ++ tx_dly_val = dwmac->tx_delay_ns >> 1; ++ /* fall through */ ++ ++ case PHY_INTERFACE_MODE_RGMII_ID: + case PHY_INTERFACE_MODE_RGMII_TXID: + /* Generate a 25MHz clock for the PHY */ + clk_rate = 25 * 1000 * 1000; +@@ -196,9 +202,8 @@ static int meson8b_init_prg_eth(struct m + meson8b_dwmac_mask_bits(dwmac, PRG_ETH0, + PRG_ETH0_INVERTED_RMII_CLK, 0); + +- /* TX clock delay - all known boards use a 1/4 cycle delay */ + meson8b_dwmac_mask_bits(dwmac, PRG_ETH0, PRG_ETH0_TXDLY_MASK, +- PRG_ETH0_TXDLY_QUARTER); ++ tx_dly_val << PRG_ETH0_TXDLY_SHIFT); + break; + + case PHY_INTERFACE_MODE_RMII: +@@ -284,6 +289,11 @@ static int meson8b_dwmac_probe(struct pl + goto err_remove_config_dt; + } + ++ /* use 2ns as fallback since this value was previously hardcoded */ ++ if (of_property_read_u32(pdev->dev.of_node, "amlogic,tx-delay-ns", ++ &dwmac->tx_delay_ns)) ++ dwmac->tx_delay_ns = 2; ++ + ret = meson8b_init_clk(dwmac); + if (ret) + goto err_remove_config_dt; +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c +@@ -302,6 +302,122 @@ static const struct rk_gmac_ops rk3288_o + .set_rmii_speed = rk3288_set_rmii_speed, + }; + ++#define RK3328_GRF_MAC_CON0 0x0900 ++#define RK3328_GRF_MAC_CON1 0x0904 ++ ++/* RK3328_GRF_MAC_CON0 */ ++#define RK3328_GMAC_CLK_RX_DL_CFG(val) HIWORD_UPDATE(val, 0x7F, 7) ++#define RK3328_GMAC_CLK_TX_DL_CFG(val) HIWORD_UPDATE(val, 0x7F, 0) ++ ++/* RK3328_GRF_MAC_CON1 */ ++#define RK3328_GMAC_PHY_INTF_SEL_RGMII \ ++ (GRF_BIT(4) | GRF_CLR_BIT(5) | GRF_CLR_BIT(6)) ++#define RK3328_GMAC_PHY_INTF_SEL_RMII \ ++ (GRF_CLR_BIT(4) | GRF_CLR_BIT(5) | GRF_BIT(6)) ++#define RK3328_GMAC_FLOW_CTRL GRF_BIT(3) ++#define RK3328_GMAC_FLOW_CTRL_CLR GRF_CLR_BIT(3) ++#define RK3328_GMAC_SPEED_10M GRF_CLR_BIT(2) ++#define RK3328_GMAC_SPEED_100M GRF_BIT(2) ++#define RK3328_GMAC_RMII_CLK_25M GRF_BIT(7) ++#define RK3328_GMAC_RMII_CLK_2_5M GRF_CLR_BIT(7) ++#define RK3328_GMAC_CLK_125M (GRF_CLR_BIT(11) | GRF_CLR_BIT(12)) ++#define RK3328_GMAC_CLK_25M (GRF_BIT(11) | GRF_BIT(12)) ++#define RK3328_GMAC_CLK_2_5M (GRF_CLR_BIT(11) | GRF_BIT(12)) ++#define RK3328_GMAC_RMII_MODE GRF_BIT(9) ++#define RK3328_GMAC_RMII_MODE_CLR GRF_CLR_BIT(9) ++#define RK3328_GMAC_TXCLK_DLY_ENABLE GRF_BIT(0) ++#define RK3328_GMAC_TXCLK_DLY_DISABLE GRF_CLR_BIT(0) ++#define RK3328_GMAC_RXCLK_DLY_ENABLE GRF_BIT(1) ++#define RK3328_GMAC_RXCLK_DLY_DISABLE GRF_CLR_BIT(0) ++ ++static void rk3328_set_to_rgmii(struct rk_priv_data *bsp_priv, ++ int tx_delay, int rx_delay) ++{ ++ struct device *dev = &bsp_priv->pdev->dev; ++ ++ if (IS_ERR(bsp_priv->grf)) { ++ dev_err(dev, "Missing rockchip,grf property\n"); ++ return; ++ } ++ ++ regmap_write(bsp_priv->grf, RK3328_GRF_MAC_CON1, ++ RK3328_GMAC_PHY_INTF_SEL_RGMII | ++ RK3328_GMAC_RMII_MODE_CLR | ++ RK3328_GMAC_RXCLK_DLY_ENABLE | ++ RK3328_GMAC_TXCLK_DLY_ENABLE); ++ ++ regmap_write(bsp_priv->grf, RK3328_GRF_MAC_CON0, ++ RK3328_GMAC_CLK_RX_DL_CFG(rx_delay) | ++ RK3328_GMAC_CLK_TX_DL_CFG(tx_delay)); ++} ++ ++static void rk3328_set_to_rmii(struct rk_priv_data *bsp_priv) ++{ ++ struct device *dev = &bsp_priv->pdev->dev; ++ ++ if (IS_ERR(bsp_priv->grf)) { ++ dev_err(dev, "Missing rockchip,grf property\n"); ++ return; ++ } ++ ++ regmap_write(bsp_priv->grf, RK3328_GRF_MAC_CON1, ++ RK3328_GMAC_PHY_INTF_SEL_RMII | ++ RK3328_GMAC_RMII_MODE); ++ ++ /* set MAC to RMII mode */ ++ regmap_write(bsp_priv->grf, RK3328_GRF_MAC_CON1, GRF_BIT(11)); ++} ++ ++static void rk3328_set_rgmii_speed(struct rk_priv_data *bsp_priv, int speed) ++{ ++ struct device *dev = &bsp_priv->pdev->dev; ++ ++ if (IS_ERR(bsp_priv->grf)) { ++ dev_err(dev, "Missing rockchip,grf property\n"); ++ return; ++ } ++ ++ if (speed == 10) ++ regmap_write(bsp_priv->grf, RK3328_GRF_MAC_CON1, ++ RK3328_GMAC_CLK_2_5M); ++ else if (speed == 100) ++ regmap_write(bsp_priv->grf, RK3328_GRF_MAC_CON1, ++ RK3328_GMAC_CLK_25M); ++ else if (speed == 1000) ++ regmap_write(bsp_priv->grf, RK3328_GRF_MAC_CON1, ++ RK3328_GMAC_CLK_125M); ++ else ++ dev_err(dev, "unknown speed value for RGMII! speed=%d", speed); ++} ++ ++static void rk3328_set_rmii_speed(struct rk_priv_data *bsp_priv, int speed) ++{ ++ struct device *dev = &bsp_priv->pdev->dev; ++ ++ if (IS_ERR(bsp_priv->grf)) { ++ dev_err(dev, "Missing rockchip,grf property\n"); ++ return; ++ } ++ ++ if (speed == 10) ++ regmap_write(bsp_priv->grf, RK3328_GRF_MAC_CON1, ++ RK3328_GMAC_RMII_CLK_2_5M | ++ RK3328_GMAC_SPEED_10M); ++ else if (speed == 100) ++ regmap_write(bsp_priv->grf, RK3328_GRF_MAC_CON1, ++ RK3328_GMAC_RMII_CLK_25M | ++ RK3328_GMAC_SPEED_100M); ++ else ++ dev_err(dev, "unknown speed value for RMII! speed=%d", speed); ++} ++ ++static const struct rk_gmac_ops rk3328_ops = { ++ .set_to_rgmii = rk3328_set_to_rgmii, ++ .set_to_rmii = rk3328_set_to_rmii, ++ .set_rgmii_speed = rk3328_set_rgmii_speed, ++ .set_rmii_speed = rk3328_set_rmii_speed, ++}; ++ + #define RK3366_GRF_SOC_CON6 0x0418 + #define RK3366_GRF_SOC_CON7 0x041c + +@@ -1006,6 +1122,7 @@ static SIMPLE_DEV_PM_OPS(rk_gmac_pm_ops, + static const struct of_device_id rk_gmac_dwmac_match[] = { + { .compatible = "rockchip,rk3228-gmac", .data = &rk3228_ops }, + { .compatible = "rockchip,rk3288-gmac", .data = &rk3288_ops }, ++ { .compatible = "rockchip,rk3328-gmac", .data = &rk3328_ops }, + { .compatible = "rockchip,rk3366-gmac", .data = &rk3366_ops }, + { .compatible = "rockchip,rk3368-gmac", .data = &rk3368_ops }, + { .compatible = "rockchip,rk3399-gmac", .data = &rk3399_ops }, +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c +@@ -341,7 +341,7 @@ static int socfpga_dwmac_probe(struct pl + * mode. Create a copy of the core reset handle so it can be used by + * the driver later. + */ +- dwmac->stmmac_rst = stpriv->stmmac_rst; ++ dwmac->stmmac_rst = stpriv->plat->stmmac_rst; + + ret = socfpga_dwmac_set_phy_mode(dwmac); + if (ret) +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100.h ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100.h +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h +@@ -10,10 +10,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c +@@ -16,10 +16,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -96,17 +92,13 @@ static int dwmac1000_rx_ipc_enable(struc + return !!(value & GMAC_CONTROL_IPC); + } + +-static void dwmac1000_dump_regs(struct mac_device_info *hw) ++static void dwmac1000_dump_regs(struct mac_device_info *hw, u32 *reg_space) + { + void __iomem *ioaddr = hw->pcsr; + int i; +- pr_info("\tDWMAC1000 regs (base addr = 0x%p)\n", ioaddr); + +- for (i = 0; i < 55; i++) { +- int offset = i * 4; +- pr_info("\tReg No. %d (offset 0x%x): 0x%08x\n", i, +- offset, readl(ioaddr + offset)); +- } ++ for (i = 0; i < 55; i++) ++ reg_space[i] = readl(ioaddr + i * 4); + } + + static void dwmac1000_set_umac_addr(struct mac_device_info *hw, +@@ -347,11 +339,14 @@ static int dwmac1000_irq_status(struct m + return ret; + } + +-static void dwmac1000_set_eee_mode(struct mac_device_info *hw) ++static void dwmac1000_set_eee_mode(struct mac_device_info *hw, ++ bool en_tx_lpi_clockgating) + { + void __iomem *ioaddr = hw->pcsr; + u32 value; + ++ /*TODO - en_tx_lpi_clockgating treatment */ ++ + /* Enable the link status receive on RGMII, SGMII ore SMII + * receive path and instruct the transmit to enter in LPI + * state. +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c +@@ -16,10 +16,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -205,18 +201,14 @@ static void dwmac1000_dma_operation_mode + writel(csr6, ioaddr + DMA_CONTROL); + } + +-static void dwmac1000_dump_dma_regs(void __iomem *ioaddr) ++static void dwmac1000_dump_dma_regs(void __iomem *ioaddr, u32 *reg_space) + { + int i; +- pr_info(" DMA registers\n"); +- for (i = 0; i < 22; i++) { +- if ((i < 9) || (i > 17)) { +- int offset = i * 4; +- pr_err("\t Reg No. %d (offset 0x%x): 0x%08x\n", i, +- (DMA_BUS_MODE + offset), +- readl(ioaddr + DMA_BUS_MODE + offset)); +- } +- } ++ ++ for (i = 0; i < 22; i++) ++ if ((i < 9) || (i > 17)) ++ reg_space[DMA_BUS_MODE / 4 + i] = ++ readl(ioaddr + DMA_BUS_MODE + i * 4); + } + + static void dwmac1000_get_hw_feature(void __iomem *ioaddr, +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c +@@ -18,10 +18,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -44,28 +40,18 @@ static void dwmac100_core_init(struct ma + #endif + } + +-static void dwmac100_dump_mac_regs(struct mac_device_info *hw) ++static void dwmac100_dump_mac_regs(struct mac_device_info *hw, u32 *reg_space) + { + void __iomem *ioaddr = hw->pcsr; +- pr_info("\t----------------------------------------------\n" +- "\t DWMAC 100 CSR (base addr = 0x%p)\n" +- "\t----------------------------------------------\n", ioaddr); +- pr_info("\tcontrol reg (offset 0x%x): 0x%08x\n", MAC_CONTROL, +- readl(ioaddr + MAC_CONTROL)); +- pr_info("\taddr HI (offset 0x%x): 0x%08x\n ", MAC_ADDR_HIGH, +- readl(ioaddr + MAC_ADDR_HIGH)); +- pr_info("\taddr LO (offset 0x%x): 0x%08x\n", MAC_ADDR_LOW, +- readl(ioaddr + MAC_ADDR_LOW)); +- pr_info("\tmulticast hash HI (offset 0x%x): 0x%08x\n", +- MAC_HASH_HIGH, readl(ioaddr + MAC_HASH_HIGH)); +- pr_info("\tmulticast hash LO (offset 0x%x): 0x%08x\n", +- MAC_HASH_LOW, readl(ioaddr + MAC_HASH_LOW)); +- pr_info("\tflow control (offset 0x%x): 0x%08x\n", +- MAC_FLOW_CTRL, readl(ioaddr + MAC_FLOW_CTRL)); +- pr_info("\tVLAN1 tag (offset 0x%x): 0x%08x\n", MAC_VLAN1, +- readl(ioaddr + MAC_VLAN1)); +- pr_info("\tVLAN2 tag (offset 0x%x): 0x%08x\n", MAC_VLAN2, +- readl(ioaddr + MAC_VLAN2)); ++ ++ reg_space[MAC_CONTROL / 4] = readl(ioaddr + MAC_CONTROL); ++ reg_space[MAC_ADDR_HIGH / 4] = readl(ioaddr + MAC_ADDR_HIGH); ++ reg_space[MAC_ADDR_LOW / 4] = readl(ioaddr + MAC_ADDR_LOW); ++ reg_space[MAC_HASH_HIGH / 4] = readl(ioaddr + MAC_HASH_HIGH); ++ reg_space[MAC_HASH_LOW / 4] = readl(ioaddr + MAC_HASH_LOW); ++ reg_space[MAC_FLOW_CTRL / 4] = readl(ioaddr + MAC_FLOW_CTRL); ++ reg_space[MAC_VLAN1 / 4] = readl(ioaddr + MAC_VLAN1); ++ reg_space[MAC_VLAN2 / 4] = readl(ioaddr + MAC_VLAN2); + } + + static int dwmac100_rx_ipc_enable(struct mac_device_info *hw) +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100_dma.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100_dma.c +@@ -18,10 +18,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -70,19 +66,18 @@ static void dwmac100_dma_operation_mode( + writel(csr6, ioaddr + DMA_CONTROL); + } + +-static void dwmac100_dump_dma_regs(void __iomem *ioaddr) ++static void dwmac100_dump_dma_regs(void __iomem *ioaddr, u32 *reg_space) + { + int i; + +- pr_debug("DWMAC 100 DMA CSR\n"); + for (i = 0; i < 9; i++) +- pr_debug("\t CSR%d (offset 0x%x): 0x%08x\n", i, +- (DMA_BUS_MODE + i * 4), +- readl(ioaddr + DMA_BUS_MODE + i * 4)); +- +- pr_debug("\tCSR20 (0x%x): 0x%08x, CSR21 (0x%x): 0x%08x\n", +- DMA_CUR_TX_BUF_ADDR, readl(ioaddr + DMA_CUR_TX_BUF_ADDR), +- DMA_CUR_RX_BUF_ADDR, readl(ioaddr + DMA_CUR_RX_BUF_ADDR)); ++ reg_space[DMA_BUS_MODE / 4 + i] = ++ readl(ioaddr + DMA_BUS_MODE + i * 4); ++ ++ reg_space[DMA_CUR_TX_BUF_ADDR / 4] = ++ readl(ioaddr + DMA_CUR_TX_BUF_ADDR); ++ reg_space[DMA_CUR_RX_BUF_ADDR / 4] = ++ readl(ioaddr + DMA_CUR_RX_BUF_ADDR); + } + + /* DMA controller has two counters to track the number of the missed frames. */ +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4.h ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4.h +@@ -22,6 +22,7 @@ + #define GMAC_HASH_TAB_32_63 0x00000014 + #define GMAC_RX_FLOW_CTRL 0x00000090 + #define GMAC_QX_TX_FLOW_CTRL(x) (0x70 + x * 4) ++#define GMAC_RXQ_CTRL0 0x000000a0 + #define GMAC_INT_STATUS 0x000000b0 + #define GMAC_INT_EN 0x000000b4 + #define GMAC_PCS_BASE 0x000000e0 +@@ -44,6 +45,11 @@ + + #define GMAC_MAX_PERFECT_ADDRESSES 128 + ++/* MAC RX Queue Enable */ ++#define GMAC_RX_QUEUE_CLEAR(queue) ~(GENMASK(1, 0) << ((queue) * 2)) ++#define GMAC_RX_AV_QUEUE_ENABLE(queue) BIT((queue) * 2) ++#define GMAC_RX_DCB_QUEUE_ENABLE(queue) BIT(((queue) * 2) + 1) ++ + /* MAC Flow Control RX */ + #define GMAC_RX_FLOW_CTRL_RFE BIT(0) + +@@ -84,6 +90,19 @@ enum power_event { + power_down = 0x00000001, + }; + ++/* Energy Efficient Ethernet (EEE) for GMAC4 ++ * ++ * LPI status, timer and control register offset ++ */ ++#define GMAC4_LPI_CTRL_STATUS 0xd0 ++#define GMAC4_LPI_TIMER_CTRL 0xd4 ++ ++/* LPI control and status defines */ ++#define GMAC4_LPI_CTRL_STATUS_LPITCSE BIT(21) /* LPI Tx Clock Stop Enable */ ++#define GMAC4_LPI_CTRL_STATUS_LPITXA BIT(19) /* Enable LPI TX Automate */ ++#define GMAC4_LPI_CTRL_STATUS_PLS BIT(17) /* PHY Link Status */ ++#define GMAC4_LPI_CTRL_STATUS_LPIEN BIT(16) /* LPI Enable */ ++ + /* MAC Debug bitmap */ + #define GMAC_DEBUG_TFCSTS_MASK GENMASK(18, 17) + #define GMAC_DEBUG_TFCSTS_SHIFT 17 +@@ -133,6 +152,8 @@ enum power_event { + /* MAC HW features2 bitmap */ + #define GMAC_HW_FEAT_TXCHCNT GENMASK(21, 18) + #define GMAC_HW_FEAT_RXCHCNT GENMASK(15, 12) ++#define GMAC_HW_FEAT_TXQCNT GENMASK(9, 6) ++#define GMAC_HW_FEAT_RXQCNT GENMASK(3, 0) + + /* MAC HW ADDR regs */ + #define GMAC_HI_DCS GENMASK(18, 16) +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c +@@ -59,19 +59,24 @@ static void dwmac4_core_init(struct mac_ + writel(value, ioaddr + GMAC_INT_EN); + } + +-static void dwmac4_dump_regs(struct mac_device_info *hw) ++static void dwmac4_rx_queue_enable(struct mac_device_info *hw, u32 queue) + { + void __iomem *ioaddr = hw->pcsr; +- int i; ++ u32 value = readl(ioaddr + GMAC_RXQ_CTRL0); + +- pr_debug("\tDWMAC4 regs (base addr = 0x%p)\n", ioaddr); ++ value &= GMAC_RX_QUEUE_CLEAR(queue); ++ value |= GMAC_RX_AV_QUEUE_ENABLE(queue); + +- for (i = 0; i < GMAC_REG_NUM; i++) { +- int offset = i * 4; ++ writel(value, ioaddr + GMAC_RXQ_CTRL0); ++} + +- pr_debug("\tReg No. %d (offset 0x%x): 0x%08x\n", i, +- offset, readl(ioaddr + offset)); +- } ++static void dwmac4_dump_regs(struct mac_device_info *hw, u32 *reg_space) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ int i; ++ ++ for (i = 0; i < GMAC_REG_NUM; i++) ++ reg_space[i] = readl(ioaddr + i * 4); + } + + static int dwmac4_rx_ipc_enable(struct mac_device_info *hw) +@@ -126,6 +131,65 @@ static void dwmac4_get_umac_addr(struct + GMAC_ADDR_LOW(reg_n)); + } + ++static void dwmac4_set_eee_mode(struct mac_device_info *hw, ++ bool en_tx_lpi_clockgating) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 value; ++ ++ /* Enable the link status receive on RGMII, SGMII ore SMII ++ * receive path and instruct the transmit to enter in LPI ++ * state. ++ */ ++ value = readl(ioaddr + GMAC4_LPI_CTRL_STATUS); ++ value |= GMAC4_LPI_CTRL_STATUS_LPIEN | GMAC4_LPI_CTRL_STATUS_LPITXA; ++ ++ if (en_tx_lpi_clockgating) ++ value |= GMAC4_LPI_CTRL_STATUS_LPITCSE; ++ ++ writel(value, ioaddr + GMAC4_LPI_CTRL_STATUS); ++} ++ ++static void dwmac4_reset_eee_mode(struct mac_device_info *hw) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 value; ++ ++ value = readl(ioaddr + GMAC4_LPI_CTRL_STATUS); ++ value &= ~(GMAC4_LPI_CTRL_STATUS_LPIEN | GMAC4_LPI_CTRL_STATUS_LPITXA); ++ writel(value, ioaddr + GMAC4_LPI_CTRL_STATUS); ++} ++ ++static void dwmac4_set_eee_pls(struct mac_device_info *hw, int link) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 value; ++ ++ value = readl(ioaddr + GMAC4_LPI_CTRL_STATUS); ++ ++ if (link) ++ value |= GMAC4_LPI_CTRL_STATUS_PLS; ++ else ++ value &= ~GMAC4_LPI_CTRL_STATUS_PLS; ++ ++ writel(value, ioaddr + GMAC4_LPI_CTRL_STATUS); ++} ++ ++static void dwmac4_set_eee_timer(struct mac_device_info *hw, int ls, int tw) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ int value = ((tw & 0xffff)) | ((ls & 0x3ff) << 16); ++ ++ /* Program the timers in the LPI timer control register: ++ * LS: minimum time (ms) for which the link ++ * status from PHY should be ok before transmitting ++ * the LPI pattern. ++ * TW: minimum time (us) for which the core waits ++ * after it has stopped transmitting the LPI pattern. ++ */ ++ writel(value, ioaddr + GMAC4_LPI_TIMER_CTRL); ++} ++ + static void dwmac4_set_filter(struct mac_device_info *hw, + struct net_device *dev) + { +@@ -392,12 +456,17 @@ static void dwmac4_debug(void __iomem *i + static const struct stmmac_ops dwmac4_ops = { + .core_init = dwmac4_core_init, + .rx_ipc = dwmac4_rx_ipc_enable, ++ .rx_queue_enable = dwmac4_rx_queue_enable, + .dump_regs = dwmac4_dump_regs, + .host_irq_status = dwmac4_irq_status, + .flow_ctrl = dwmac4_flow_ctrl, + .pmt = dwmac4_pmt, + .set_umac_addr = dwmac4_set_umac_addr, + .get_umac_addr = dwmac4_get_umac_addr, ++ .set_eee_mode = dwmac4_set_eee_mode, ++ .reset_eee_mode = dwmac4_reset_eee_mode, ++ .set_eee_timer = dwmac4_set_eee_timer, ++ .set_eee_pls = dwmac4_set_eee_pls, + .pcs_ctrl_ane = dwmac4_ctrl_ane, + .pcs_rane = dwmac4_rane, + .pcs_get_adv_lp = dwmac4_get_adv_lp, +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c +@@ -103,7 +103,7 @@ static int dwmac4_wrback_get_rx_status(v + x->rx_mii++; + + if (unlikely(rdes3 & RDES3_CRC_ERROR)) { +- x->rx_crc++; ++ x->rx_crc_errors++; + stats->rx_crc_errors++; + } + +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c +@@ -127,53 +127,51 @@ static void dwmac4_dma_init(void __iomem + dwmac4_dma_init_channel(ioaddr, dma_cfg, dma_tx, dma_rx, i); + } + +-static void _dwmac4_dump_dma_regs(void __iomem *ioaddr, u32 channel) ++static void _dwmac4_dump_dma_regs(void __iomem *ioaddr, u32 channel, ++ u32 *reg_space) + { +- pr_debug(" Channel %d\n", channel); +- pr_debug("\tDMA_CHAN_CONTROL, offset: 0x%x, val: 0x%x\n", 0, +- readl(ioaddr + DMA_CHAN_CONTROL(channel))); +- pr_debug("\tDMA_CHAN_TX_CONTROL, offset: 0x%x, val: 0x%x\n", 0x4, +- readl(ioaddr + DMA_CHAN_TX_CONTROL(channel))); +- pr_debug("\tDMA_CHAN_RX_CONTROL, offset: 0x%x, val: 0x%x\n", 0x8, +- readl(ioaddr + DMA_CHAN_RX_CONTROL(channel))); +- pr_debug("\tDMA_CHAN_TX_BASE_ADDR, offset: 0x%x, val: 0x%x\n", 0x14, +- readl(ioaddr + DMA_CHAN_TX_BASE_ADDR(channel))); +- pr_debug("\tDMA_CHAN_RX_BASE_ADDR, offset: 0x%x, val: 0x%x\n", 0x1c, +- readl(ioaddr + DMA_CHAN_RX_BASE_ADDR(channel))); +- pr_debug("\tDMA_CHAN_TX_END_ADDR, offset: 0x%x, val: 0x%x\n", 0x20, +- readl(ioaddr + DMA_CHAN_TX_END_ADDR(channel))); +- pr_debug("\tDMA_CHAN_RX_END_ADDR, offset: 0x%x, val: 0x%x\n", 0x28, +- readl(ioaddr + DMA_CHAN_RX_END_ADDR(channel))); +- pr_debug("\tDMA_CHAN_TX_RING_LEN, offset: 0x%x, val: 0x%x\n", 0x2c, +- readl(ioaddr + DMA_CHAN_TX_RING_LEN(channel))); +- pr_debug("\tDMA_CHAN_RX_RING_LEN, offset: 0x%x, val: 0x%x\n", 0x30, +- readl(ioaddr + DMA_CHAN_RX_RING_LEN(channel))); +- pr_debug("\tDMA_CHAN_INTR_ENA, offset: 0x%x, val: 0x%x\n", 0x34, +- readl(ioaddr + DMA_CHAN_INTR_ENA(channel))); +- pr_debug("\tDMA_CHAN_RX_WATCHDOG, offset: 0x%x, val: 0x%x\n", 0x38, +- readl(ioaddr + DMA_CHAN_RX_WATCHDOG(channel))); +- pr_debug("\tDMA_CHAN_SLOT_CTRL_STATUS, offset: 0x%x, val: 0x%x\n", 0x3c, +- readl(ioaddr + DMA_CHAN_SLOT_CTRL_STATUS(channel))); +- pr_debug("\tDMA_CHAN_CUR_TX_DESC, offset: 0x%x, val: 0x%x\n", 0x44, +- readl(ioaddr + DMA_CHAN_CUR_TX_DESC(channel))); +- pr_debug("\tDMA_CHAN_CUR_RX_DESC, offset: 0x%x, val: 0x%x\n", 0x4c, +- readl(ioaddr + DMA_CHAN_CUR_RX_DESC(channel))); +- pr_debug("\tDMA_CHAN_CUR_TX_BUF_ADDR, offset: 0x%x, val: 0x%x\n", 0x54, +- readl(ioaddr + DMA_CHAN_CUR_TX_BUF_ADDR(channel))); +- pr_debug("\tDMA_CHAN_CUR_RX_BUF_ADDR, offset: 0x%x, val: 0x%x\n", 0x5c, +- readl(ioaddr + DMA_CHAN_CUR_RX_BUF_ADDR(channel))); +- pr_debug("\tDMA_CHAN_STATUS, offset: 0x%x, val: 0x%x\n", 0x60, +- readl(ioaddr + DMA_CHAN_STATUS(channel))); ++ reg_space[DMA_CHAN_CONTROL(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_CONTROL(channel)); ++ reg_space[DMA_CHAN_TX_CONTROL(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_TX_CONTROL(channel)); ++ reg_space[DMA_CHAN_RX_CONTROL(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_RX_CONTROL(channel)); ++ reg_space[DMA_CHAN_TX_BASE_ADDR(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_TX_BASE_ADDR(channel)); ++ reg_space[DMA_CHAN_RX_BASE_ADDR(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_RX_BASE_ADDR(channel)); ++ reg_space[DMA_CHAN_TX_END_ADDR(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_TX_END_ADDR(channel)); ++ reg_space[DMA_CHAN_RX_END_ADDR(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_RX_END_ADDR(channel)); ++ reg_space[DMA_CHAN_TX_RING_LEN(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_TX_RING_LEN(channel)); ++ reg_space[DMA_CHAN_RX_RING_LEN(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_RX_RING_LEN(channel)); ++ reg_space[DMA_CHAN_INTR_ENA(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_INTR_ENA(channel)); ++ reg_space[DMA_CHAN_RX_WATCHDOG(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_RX_WATCHDOG(channel)); ++ reg_space[DMA_CHAN_SLOT_CTRL_STATUS(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_SLOT_CTRL_STATUS(channel)); ++ reg_space[DMA_CHAN_CUR_TX_DESC(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_CUR_TX_DESC(channel)); ++ reg_space[DMA_CHAN_CUR_RX_DESC(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_CUR_RX_DESC(channel)); ++ reg_space[DMA_CHAN_CUR_TX_BUF_ADDR(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_CUR_TX_BUF_ADDR(channel)); ++ reg_space[DMA_CHAN_CUR_RX_BUF_ADDR(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_CUR_RX_BUF_ADDR(channel)); ++ reg_space[DMA_CHAN_STATUS(channel) / 4] = ++ readl(ioaddr + DMA_CHAN_STATUS(channel)); + } + +-static void dwmac4_dump_dma_regs(void __iomem *ioaddr) ++static void dwmac4_dump_dma_regs(void __iomem *ioaddr, u32 *reg_space) + { + int i; + +- pr_debug(" GMAC4 DMA registers\n"); +- + for (i = 0; i < DMA_CHANNEL_NB_MAX; i++) +- _dwmac4_dump_dma_regs(ioaddr, i); ++ _dwmac4_dump_dma_regs(ioaddr, i, reg_space); + } + + static void dwmac4_rx_watchdog(void __iomem *ioaddr, u32 riwt) +@@ -303,6 +301,11 @@ static void dwmac4_get_hw_feature(void _ + ((hw_cap & GMAC_HW_FEAT_RXCHCNT) >> 12) + 1; + dma_cap->number_tx_channel = + ((hw_cap & GMAC_HW_FEAT_TXCHCNT) >> 18) + 1; ++ /* TX and RX number of queues */ ++ dma_cap->number_rx_queues = ++ ((hw_cap & GMAC_HW_FEAT_RXQCNT) >> 0) + 1; ++ dma_cap->number_tx_queues = ++ ((hw_cap & GMAC_HW_FEAT_TXQCNT) >> 6) + 1; + + /* IEEE 1588-2002 */ + dma_cap->time_stamp = 0; +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c +@@ -10,10 +10,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -21,6 +17,7 @@ + *******************************************************************************/ + + #include ++#include + #include "common.h" + #include "dwmac_dma.h" + +@@ -29,19 +26,16 @@ + int dwmac_dma_reset(void __iomem *ioaddr) + { + u32 value = readl(ioaddr + DMA_BUS_MODE); +- int limit; ++ int err; + + /* DMA SW reset */ + value |= DMA_BUS_MODE_SFT_RESET; + writel(value, ioaddr + DMA_BUS_MODE); +- limit = 10; +- while (limit--) { +- if (!(readl(ioaddr + DMA_BUS_MODE) & DMA_BUS_MODE_SFT_RESET)) +- break; +- mdelay(10); +- } + +- if (limit < 0) ++ err = readl_poll_timeout(ioaddr + DMA_BUS_MODE, value, ++ !(value & DMA_BUS_MODE_SFT_RESET), ++ 100000, 10000); ++ if (err) + return -EBUSY; + + return 0; +@@ -102,7 +96,7 @@ static void show_tx_process_state(unsign + pr_debug("- TX (Stopped): Reset or Stop command\n"); + break; + case 1: +- pr_debug("- TX (Running):Fetching the Tx desc\n"); ++ pr_debug("- TX (Running): Fetching the Tx desc\n"); + break; + case 2: + pr_debug("- TX (Running): Waiting for end of tx\n"); +@@ -136,7 +130,7 @@ static void show_rx_process_state(unsign + pr_debug("- RX (Running): Fetching the Rx desc\n"); + break; + case 2: +- pr_debug("- RX (Running):Checking for end of pkt\n"); ++ pr_debug("- RX (Running): Checking for end of pkt\n"); + break; + case 3: + pr_debug("- RX (Running): Waiting for Rx pkt\n"); +@@ -246,7 +240,7 @@ void stmmac_set_mac_addr(void __iomem *i + unsigned long data; + + data = (addr[5] << 8) | addr[4]; +- /* For MAC Addr registers se have to set the Address Enable (AE) ++ /* For MAC Addr registers we have to set the Address Enable (AE) + * bit that has no effect on the High Reg 0 where the bit 31 (MO) + * is RO. + */ +@@ -261,9 +255,9 @@ void stmmac_set_mac(void __iomem *ioaddr + u32 value = readl(ioaddr + MAC_CTRL_REG); + + if (enable) +- value |= MAC_RNABLE_RX | MAC_ENABLE_TX; ++ value |= MAC_ENABLE_RX | MAC_ENABLE_TX; + else +- value &= ~(MAC_ENABLE_TX | MAC_RNABLE_RX); ++ value &= ~(MAC_ENABLE_TX | MAC_ENABLE_RX); + + writel(value, ioaddr + MAC_CTRL_REG); + } +--- a/drivers/net/ethernet/stmicro/stmmac/enh_desc.c ++++ b/drivers/net/ethernet/stmicro/stmmac/enh_desc.c +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -225,7 +221,7 @@ static int enh_desc_get_rx_status(void * + x->rx_mii++; + + if (unlikely(rdes0 & RDES0_CRC_ERROR)) { +- x->rx_crc++; ++ x->rx_crc_errors++; + stats->rx_crc_errors++; + } + ret = discard_frame; +--- a/drivers/net/ethernet/stmicro/stmmac/mmc.h ++++ b/drivers/net/ethernet/stmicro/stmmac/mmc.h +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- a/drivers/net/ethernet/stmicro/stmmac/mmc_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/mmc_core.c +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- a/drivers/net/ethernet/stmicro/stmmac/norm_desc.c ++++ b/drivers/net/ethernet/stmicro/stmmac/norm_desc.c +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -115,7 +111,7 @@ static int ndesc_get_rx_status(void *dat + stats->collisions++; + } + if (unlikely(rdes0 & RDES0_CRC_ERROR)) { +- x->rx_crc++; ++ x->rx_crc_errors++; + stats->rx_crc_errors++; + } + ret = discard_frame; +--- a/drivers/net/ethernet/stmicro/stmmac/ring_mode.c ++++ b/drivers/net/ethernet/stmicro/stmmac/ring_mode.c +@@ -16,10 +16,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h +@@ -10,10 +10,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -106,9 +102,6 @@ struct stmmac_priv { + u32 msg_enable; + int wolopts; + int wol_irq; +- struct clk *stmmac_clk; +- struct clk *pclk; +- struct reset_control *stmmac_rst; + int clk_csr; + struct timer_list eee_ctrl_timer; + int lpi_irq; +@@ -120,8 +113,6 @@ struct stmmac_priv { + struct ptp_clock *ptp_clock; + struct ptp_clock_info ptp_clock_ops; + unsigned int default_addend; +- struct clk *clk_ptp_ref; +- unsigned int clk_ptp_rate; + u32 adv_ts; + int use_riwt; + int irq_wake; +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -65,7 +61,7 @@ static const struct stmmac_stats stmmac_ + STMMAC_STAT(overflow_error), + STMMAC_STAT(ipc_csum_error), + STMMAC_STAT(rx_collision), +- STMMAC_STAT(rx_crc), ++ STMMAC_STAT(rx_crc_errors), + STMMAC_STAT(dribbling_bit), + STMMAC_STAT(rx_length), + STMMAC_STAT(rx_mii), +@@ -439,32 +435,14 @@ static int stmmac_ethtool_get_regs_len(s + static void stmmac_ethtool_gregs(struct net_device *dev, + struct ethtool_regs *regs, void *space) + { +- int i; + u32 *reg_space = (u32 *) space; + + struct stmmac_priv *priv = netdev_priv(dev); + + memset(reg_space, 0x0, REG_SPACE_SIZE); + +- if (!(priv->plat->has_gmac || priv->plat->has_gmac4)) { +- /* MAC registers */ +- for (i = 0; i < 12; i++) +- reg_space[i] = readl(priv->ioaddr + (i * 4)); +- /* DMA registers */ +- for (i = 0; i < 9; i++) +- reg_space[i + 12] = +- readl(priv->ioaddr + (DMA_BUS_MODE + (i * 4))); +- reg_space[22] = readl(priv->ioaddr + DMA_CUR_TX_BUF_ADDR); +- reg_space[23] = readl(priv->ioaddr + DMA_CUR_RX_BUF_ADDR); +- } else { +- /* MAC registers */ +- for (i = 0; i < 55; i++) +- reg_space[i] = readl(priv->ioaddr + (i * 4)); +- /* DMA registers */ +- for (i = 0; i < 22; i++) +- reg_space[i + 55] = +- readl(priv->ioaddr + (DMA_BUS_MODE + (i * 4))); +- } ++ priv->hw->mac->dump_regs(priv->hw, reg_space); ++ priv->hw->dma->dump_regs(priv->ioaddr, reg_space); + } + + static void +@@ -712,7 +690,7 @@ static int stmmac_ethtool_op_set_eee(str + + static u32 stmmac_usec2riwt(u32 usec, struct stmmac_priv *priv) + { +- unsigned long clk = clk_get_rate(priv->stmmac_clk); ++ unsigned long clk = clk_get_rate(priv->plat->stmmac_clk); + + if (!clk) + return 0; +@@ -722,7 +700,7 @@ static u32 stmmac_usec2riwt(u32 usec, st + + static u32 stmmac_riwt2usec(u32 riwt, struct stmmac_priv *priv) + { +- unsigned long clk = clk_get_rate(priv->stmmac_clk); ++ unsigned long clk = clk_get_rate(priv->plat->stmmac_clk); + + if (!clk) + return 0; +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +@@ -13,10 +13,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -158,7 +154,7 @@ static void stmmac_clk_csr_set(struct st + { + u32 clk_rate; + +- clk_rate = clk_get_rate(priv->stmmac_clk); ++ clk_rate = clk_get_rate(priv->plat->stmmac_clk); + + /* Platform provided default clk_csr would be assumed valid + * for all other cases except for the below mentioned ones. +@@ -191,7 +187,7 @@ static void print_pkt(unsigned char *buf + + static inline u32 stmmac_tx_avail(struct stmmac_priv *priv) + { +- unsigned avail; ++ u32 avail; + + if (priv->dirty_tx > priv->cur_tx) + avail = priv->dirty_tx - priv->cur_tx - 1; +@@ -203,7 +199,7 @@ static inline u32 stmmac_tx_avail(struct + + static inline u32 stmmac_rx_dirty(struct stmmac_priv *priv) + { +- unsigned dirty; ++ u32 dirty; + + if (priv->dirty_rx <= priv->cur_rx) + dirty = priv->cur_rx - priv->dirty_rx; +@@ -216,7 +212,7 @@ static inline u32 stmmac_rx_dirty(struct + /** + * stmmac_hw_fix_mac_speed - callback for speed selection + * @priv: driver private structure +- * Description: on some platforms (e.g. ST), some HW system configuraton ++ * Description: on some platforms (e.g. ST), some HW system configuration + * registers have to be set according to the link speed negotiated. + */ + static inline void stmmac_hw_fix_mac_speed(struct stmmac_priv *priv) +@@ -239,7 +235,8 @@ static void stmmac_enable_eee_mode(struc + /* Check and enter in LPI mode */ + if ((priv->dirty_tx == priv->cur_tx) && + (priv->tx_path_in_lpi_mode == false)) +- priv->hw->mac->set_eee_mode(priv->hw); ++ priv->hw->mac->set_eee_mode(priv->hw, ++ priv->plat->en_tx_lpi_clockgating); + } + + /** +@@ -415,7 +412,7 @@ static void stmmac_get_rx_hwtstamp(struc + /** + * stmmac_hwtstamp_ioctl - control hardware timestamping. + * @dev: device pointer. +- * @ifr: An IOCTL specefic structure, that can contain a pointer to ++ * @ifr: An IOCTL specific structure, that can contain a pointer to + * a proprietary structure used to pass information to the driver. + * Description: + * This function configures the MAC to enable/disable both outgoing(TX) +@@ -606,7 +603,7 @@ static int stmmac_hwtstamp_ioctl(struct + + /* program Sub Second Increment reg */ + sec_inc = priv->hw->ptp->config_sub_second_increment( +- priv->ptpaddr, priv->clk_ptp_rate, ++ priv->ptpaddr, priv->plat->clk_ptp_rate, + priv->plat->has_gmac4); + temp = div_u64(1000000000ULL, sec_inc); + +@@ -616,7 +613,7 @@ static int stmmac_hwtstamp_ioctl(struct + * where, freq_div_ratio = 1e9ns/sec_inc + */ + temp = (u64)(temp << 32); +- priv->default_addend = div_u64(temp, priv->clk_ptp_rate); ++ priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate); + priv->hw->ptp->config_addend(priv->ptpaddr, + priv->default_addend); + +@@ -644,18 +641,6 @@ static int stmmac_init_ptp(struct stmmac + if (!(priv->dma_cap.time_stamp || priv->dma_cap.atime_stamp)) + return -EOPNOTSUPP; + +- /* Fall-back to main clock in case of no PTP ref is passed */ +- priv->clk_ptp_ref = devm_clk_get(priv->device, "clk_ptp_ref"); +- if (IS_ERR(priv->clk_ptp_ref)) { +- priv->clk_ptp_rate = clk_get_rate(priv->stmmac_clk); +- priv->clk_ptp_ref = NULL; +- netdev_dbg(priv->dev, "PTP uses main clock\n"); +- } else { +- clk_prepare_enable(priv->clk_ptp_ref); +- priv->clk_ptp_rate = clk_get_rate(priv->clk_ptp_ref); +- netdev_dbg(priv->dev, "PTP rate %d\n", priv->clk_ptp_rate); +- } +- + priv->adv_ts = 0; + /* Check if adv_ts can be enabled for dwmac 4.x core */ + if (priv->plat->has_gmac4 && priv->dma_cap.atime_stamp) +@@ -682,8 +667,8 @@ static int stmmac_init_ptp(struct stmmac + + static void stmmac_release_ptp(struct stmmac_priv *priv) + { +- if (priv->clk_ptp_ref) +- clk_disable_unprepare(priv->clk_ptp_ref); ++ if (priv->plat->clk_ptp_ref) ++ clk_disable_unprepare(priv->plat->clk_ptp_ref); + stmmac_ptp_unregister(priv); + } + +@@ -704,7 +689,7 @@ static void stmmac_adjust_link(struct ne + int new_state = 0; + unsigned int fc = priv->flow_ctrl, pause_time = priv->pause; + +- if (phydev == NULL) ++ if (!phydev) + return; + + spin_lock_irqsave(&priv->lock, flags); +@@ -731,33 +716,36 @@ static void stmmac_adjust_link(struct ne + new_state = 1; + switch (phydev->speed) { + case 1000: +- if (likely((priv->plat->has_gmac) || +- (priv->plat->has_gmac4))) ++ if (priv->plat->has_gmac || ++ priv->plat->has_gmac4) + ctrl &= ~priv->hw->link.port; +- stmmac_hw_fix_mac_speed(priv); + break; + case 100: ++ if (priv->plat->has_gmac || ++ priv->plat->has_gmac4) { ++ ctrl |= priv->hw->link.port; ++ ctrl |= priv->hw->link.speed; ++ } else { ++ ctrl &= ~priv->hw->link.port; ++ } ++ break; + case 10: +- if (likely((priv->plat->has_gmac) || +- (priv->plat->has_gmac4))) { ++ if (priv->plat->has_gmac || ++ priv->plat->has_gmac4) { + ctrl |= priv->hw->link.port; +- if (phydev->speed == SPEED_100) { +- ctrl |= priv->hw->link.speed; +- } else { +- ctrl &= ~(priv->hw->link.speed); +- } ++ ctrl &= ~(priv->hw->link.speed); + } else { + ctrl &= ~priv->hw->link.port; + } +- stmmac_hw_fix_mac_speed(priv); + break; + default: + netif_warn(priv, link, priv->dev, +- "Speed (%d) not 10/100\n", +- phydev->speed); ++ "broken speed: %d\n", phydev->speed); ++ phydev->speed = SPEED_UNKNOWN; + break; + } +- ++ if (phydev->speed != SPEED_UNKNOWN) ++ stmmac_hw_fix_mac_speed(priv); + priv->speed = phydev->speed; + } + +@@ -770,8 +758,8 @@ static void stmmac_adjust_link(struct ne + } else if (priv->oldlink) { + new_state = 1; + priv->oldlink = 0; +- priv->speed = 0; +- priv->oldduplex = -1; ++ priv->speed = SPEED_UNKNOWN; ++ priv->oldduplex = DUPLEX_UNKNOWN; + } + + if (new_state && netif_msg_link(priv)) +@@ -833,8 +821,8 @@ static int stmmac_init_phy(struct net_de + int interface = priv->plat->interface; + int max_speed = priv->plat->max_speed; + priv->oldlink = 0; +- priv->speed = 0; +- priv->oldduplex = -1; ++ priv->speed = SPEED_UNKNOWN; ++ priv->oldduplex = DUPLEX_UNKNOWN; + + if (priv->plat->phy_node) { + phydev = of_phy_connect(dev, priv->plat->phy_node, +@@ -886,9 +874,7 @@ static int stmmac_init_phy(struct net_de + if (phydev->is_pseudo_fixed_link) + phydev->irq = PHY_POLL; + +- netdev_dbg(priv->dev, "%s: attached to PHY (UID 0x%x) Link = %d\n", +- __func__, phydev->phy_id, phydev->link); +- ++ phy_attached_info(phydev); + return 0; + } + +@@ -1014,7 +1000,7 @@ static void stmmac_free_rx_buffers(struc + * @dev: net device structure + * @flags: gfp flag. + * Description: this function initializes the DMA RX/TX descriptors +- * and allocates the socket buffers. It suppors the chained and ring ++ * and allocates the socket buffers. It supports the chained and ring + * modes. + */ + static int init_dma_desc_rings(struct net_device *dev, gfp_t flags) +@@ -1127,13 +1113,6 @@ static void dma_free_tx_skbufs(struct st + int i; + + for (i = 0; i < DMA_TX_SIZE; i++) { +- struct dma_desc *p; +- +- if (priv->extend_desc) +- p = &((priv->dma_etx + i)->basic); +- else +- p = priv->dma_tx + i; +- + if (priv->tx_skbuff_dma[i].buf) { + if (priv->tx_skbuff_dma[i].map_as_page) + dma_unmap_page(priv->device, +@@ -1147,7 +1126,7 @@ static void dma_free_tx_skbufs(struct st + DMA_TO_DEVICE); + } + +- if (priv->tx_skbuff[i] != NULL) { ++ if (priv->tx_skbuff[i]) { + dev_kfree_skb_any(priv->tx_skbuff[i]); + priv->tx_skbuff[i] = NULL; + priv->tx_skbuff_dma[i].buf = 0; +@@ -1271,6 +1250,28 @@ static void free_dma_desc_resources(stru + } + + /** ++ * stmmac_mac_enable_rx_queues - Enable MAC rx queues ++ * @priv: driver private structure ++ * Description: It is used for enabling the rx queues in the MAC ++ */ ++static void stmmac_mac_enable_rx_queues(struct stmmac_priv *priv) ++{ ++ int rx_count = priv->dma_cap.number_rx_queues; ++ int queue = 0; ++ ++ /* If GMAC does not have multiple queues, then this is not necessary*/ ++ if (rx_count == 1) ++ return; ++ ++ /** ++ * If the core is synthesized with multiple rx queues / multiple ++ * dma channels, then rx queues will be disabled by default. ++ * For now only rx queue 0 is enabled. ++ */ ++ priv->hw->mac->rx_queue_enable(priv->hw, queue); ++} ++ ++/** + * stmmac_dma_operation_mode - HW DMA operation mode + * @priv: driver private structure + * Description: it is used for configuring the DMA operation mode register in +@@ -1671,10 +1672,6 @@ static int stmmac_hw_setup(struct net_de + /* Copy the MAC addr into the HW */ + priv->hw->mac->set_umac_addr(priv->hw, dev->dev_addr, 0); + +- /* If required, perform hw setup of the bus. */ +- if (priv->plat->bus_setup) +- priv->plat->bus_setup(priv->ioaddr); +- + /* PS and related bits will be programmed according to the speed */ + if (priv->hw->pcs) { + int speed = priv->plat->mac_port_sel_speed; +@@ -1691,6 +1688,10 @@ static int stmmac_hw_setup(struct net_de + /* Initialize the MAC Core */ + priv->hw->mac->core_init(priv->hw, dev->mtu); + ++ /* Initialize MAC RX Queues */ ++ if (priv->hw->mac->rx_queue_enable) ++ stmmac_mac_enable_rx_queues(priv); ++ + ret = priv->hw->mac->rx_ipc(priv->hw); + if (!ret) { + netdev_warn(priv->dev, "RX IPC Checksum Offload disabled\n"); +@@ -1711,8 +1712,10 @@ static int stmmac_hw_setup(struct net_de + + if (init_ptp) { + ret = stmmac_init_ptp(priv); +- if (ret) +- netdev_warn(priv->dev, "fail to init PTP.\n"); ++ if (ret == -EOPNOTSUPP) ++ netdev_warn(priv->dev, "PTP not supported by HW\n"); ++ else if (ret) ++ netdev_warn(priv->dev, "PTP init failed\n"); + } + + #ifdef CONFIG_DEBUG_FS +@@ -1726,11 +1729,6 @@ static int stmmac_hw_setup(struct net_de + priv->hw->dma->start_tx(priv->ioaddr); + priv->hw->dma->start_rx(priv->ioaddr); + +- /* Dump DMA/MAC registers */ +- if (netif_msg_hw(priv)) { +- priv->hw->mac->dump_regs(priv->hw); +- priv->hw->dma->dump_regs(priv->ioaddr); +- } + priv->tx_lpi_timer = STMMAC_DEFAULT_TWT_LS; + + if ((priv->use_riwt) && (priv->hw->dma->rx_watchdog)) { +@@ -2519,7 +2517,7 @@ static int stmmac_rx(struct stmmac_priv + if (unlikely(status == discard_frame)) { + priv->dev->stats.rx_errors++; + if (priv->hwts_rx_en && !priv->extend_desc) { +- /* DESC2 & DESC3 will be overwitten by device ++ /* DESC2 & DESC3 will be overwritten by device + * with timestamp value, hence reinitialize + * them in stmmac_rx_refill() function so that + * device can reuse it. +@@ -2542,7 +2540,7 @@ static int stmmac_rx(struct stmmac_priv + + frame_len = priv->hw->desc->get_rx_frame_len(p, coe); + +- /* If frame length is greather than skb buffer size ++ /* If frame length is greater than skb buffer size + * (preallocated during init) then the packet is + * ignored + */ +@@ -2669,7 +2667,7 @@ static int stmmac_poll(struct napi_struc + + work_done = stmmac_rx(priv, budget); + if (work_done < budget) { +- napi_complete(napi); ++ napi_complete_done(napi, work_done); + stmmac_enable_dma_irq(priv); + } + return work_done; +@@ -2762,7 +2760,7 @@ static netdev_features_t stmmac_fix_feat + /* Some GMAC devices have a bugged Jumbo frame support that + * needs to have the Tx COE disabled for oversized frames + * (due to limited buffer sizes). In this case we disable +- * the TX csum insertionin the TDES and not use SF. ++ * the TX csum insertion in the TDES and not use SF. + */ + if (priv->plat->bugged_jumbo && (dev->mtu > ETH_DATA_LEN)) + features &= ~NETIF_F_CSUM_MASK; +@@ -2908,9 +2906,7 @@ static void sysfs_display_ring(void *hea + struct dma_desc *p = (struct dma_desc *)head; + + for (i = 0; i < size; i++) { +- u64 x; + if (extend_desc) { +- x = *(u64 *) ep; + seq_printf(seq, "%d [0x%x]: 0x%x 0x%x 0x%x 0x%x\n", + i, (unsigned int)virt_to_phys(ep), + le32_to_cpu(ep->basic.des0), +@@ -2919,7 +2915,6 @@ static void sysfs_display_ring(void *hea + le32_to_cpu(ep->basic.des3)); + ep++; + } else { +- x = *(u64 *) p; + seq_printf(seq, "%d [0x%x]: 0x%x 0x%x 0x%x 0x%x\n", + i, (unsigned int)virt_to_phys(ep), + le32_to_cpu(p->des0), le32_to_cpu(p->des1), +@@ -2989,7 +2984,7 @@ static int stmmac_sysfs_dma_cap_read(str + (priv->dma_cap.hash_filter) ? "Y" : "N"); + seq_printf(seq, "\tMultiple MAC address registers: %s\n", + (priv->dma_cap.multi_addr) ? "Y" : "N"); +- seq_printf(seq, "\tPCS (TBI/SGMII/RTBI PHY interfatces): %s\n", ++ seq_printf(seq, "\tPCS (TBI/SGMII/RTBI PHY interfaces): %s\n", + (priv->dma_cap.pcs) ? "Y" : "N"); + seq_printf(seq, "\tSMA (MDIO) Interface: %s\n", + (priv->dma_cap.sma_mdio) ? "Y" : "N"); +@@ -3265,44 +3260,8 @@ int stmmac_dvr_probe(struct device *devi + if ((phyaddr >= 0) && (phyaddr <= 31)) + priv->plat->phy_addr = phyaddr; + +- priv->stmmac_clk = devm_clk_get(priv->device, STMMAC_RESOURCE_NAME); +- if (IS_ERR(priv->stmmac_clk)) { +- netdev_warn(priv->dev, "%s: warning: cannot get CSR clock\n", +- __func__); +- /* If failed to obtain stmmac_clk and specific clk_csr value +- * is NOT passed from the platform, probe fail. +- */ +- if (!priv->plat->clk_csr) { +- ret = PTR_ERR(priv->stmmac_clk); +- goto error_clk_get; +- } else { +- priv->stmmac_clk = NULL; +- } +- } +- clk_prepare_enable(priv->stmmac_clk); +- +- priv->pclk = devm_clk_get(priv->device, "pclk"); +- if (IS_ERR(priv->pclk)) { +- if (PTR_ERR(priv->pclk) == -EPROBE_DEFER) { +- ret = -EPROBE_DEFER; +- goto error_pclk_get; +- } +- priv->pclk = NULL; +- } +- clk_prepare_enable(priv->pclk); +- +- priv->stmmac_rst = devm_reset_control_get(priv->device, +- STMMAC_RESOURCE_NAME); +- if (IS_ERR(priv->stmmac_rst)) { +- if (PTR_ERR(priv->stmmac_rst) == -EPROBE_DEFER) { +- ret = -EPROBE_DEFER; +- goto error_hw_init; +- } +- dev_info(priv->device, "no reset control found\n"); +- priv->stmmac_rst = NULL; +- } +- if (priv->stmmac_rst) +- reset_control_deassert(priv->stmmac_rst); ++ if (priv->plat->stmmac_rst) ++ reset_control_deassert(priv->plat->stmmac_rst); + + /* Init MAC and get the capabilities */ + ret = stmmac_hw_init(priv); +@@ -3388,10 +3347,6 @@ error_netdev_register: + error_mdio_register: + netif_napi_del(&priv->napi); + error_hw_init: +- clk_disable_unprepare(priv->pclk); +-error_pclk_get: +- clk_disable_unprepare(priv->stmmac_clk); +-error_clk_get: + free_netdev(ndev); + + return ret; +@@ -3417,10 +3372,10 @@ int stmmac_dvr_remove(struct device *dev + stmmac_set_mac(priv->ioaddr, false); + netif_carrier_off(ndev); + unregister_netdev(ndev); +- if (priv->stmmac_rst) +- reset_control_assert(priv->stmmac_rst); +- clk_disable_unprepare(priv->pclk); +- clk_disable_unprepare(priv->stmmac_clk); ++ if (priv->plat->stmmac_rst) ++ reset_control_assert(priv->plat->stmmac_rst); ++ clk_disable_unprepare(priv->plat->pclk); ++ clk_disable_unprepare(priv->plat->stmmac_clk); + if (priv->hw->pcs != STMMAC_PCS_RGMII && + priv->hw->pcs != STMMAC_PCS_TBI && + priv->hw->pcs != STMMAC_PCS_RTBI) +@@ -3469,14 +3424,14 @@ int stmmac_suspend(struct device *dev) + stmmac_set_mac(priv->ioaddr, false); + pinctrl_pm_select_sleep_state(priv->device); + /* Disable clock in case of PWM is off */ +- clk_disable(priv->pclk); +- clk_disable(priv->stmmac_clk); ++ clk_disable(priv->plat->pclk); ++ clk_disable(priv->plat->stmmac_clk); + } + spin_unlock_irqrestore(&priv->lock, flags); + + priv->oldlink = 0; +- priv->speed = 0; +- priv->oldduplex = -1; ++ priv->speed = SPEED_UNKNOWN; ++ priv->oldduplex = DUPLEX_UNKNOWN; + return 0; + } + EXPORT_SYMBOL_GPL(stmmac_suspend); +@@ -3509,9 +3464,9 @@ int stmmac_resume(struct device *dev) + priv->irq_wake = 0; + } else { + pinctrl_pm_select_default_state(priv->device); +- /* enable the clk prevously disabled */ +- clk_enable(priv->stmmac_clk); +- clk_enable(priv->pclk); ++ /* enable the clk previously disabled */ ++ clk_enable(priv->plat->stmmac_clk); ++ clk_enable(priv->plat->pclk); + /* reset the phy so that it's ready */ + if (priv->mii) + stmmac_mdio_reset(priv->mii); +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c +@@ -13,10 +13,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -24,13 +20,14 @@ + Maintainer: Giuseppe Cavallaro + *******************************************************************************/ + ++#include ++#include + #include +-#include +-#include + #include + #include + #include +-#include ++#include ++#include + + #include "stmmac.h" + +@@ -42,22 +39,6 @@ + #define MII_GMAC4_WRITE (1 << MII_GMAC4_GOC_SHIFT) + #define MII_GMAC4_READ (3 << MII_GMAC4_GOC_SHIFT) + +-static int stmmac_mdio_busy_wait(void __iomem *ioaddr, unsigned int mii_addr) +-{ +- unsigned long curr; +- unsigned long finish = jiffies + 3 * HZ; +- +- do { +- curr = jiffies; +- if (readl(ioaddr + mii_addr) & MII_BUSY) +- cpu_relax(); +- else +- return 0; +- } while (!time_after_eq(curr, finish)); +- +- return -EBUSY; +-} +- + /** + * stmmac_mdio_read + * @bus: points to the mii_bus structure +@@ -74,7 +55,7 @@ static int stmmac_mdio_read(struct mii_b + struct stmmac_priv *priv = netdev_priv(ndev); + unsigned int mii_address = priv->hw->mii.addr; + unsigned int mii_data = priv->hw->mii.data; +- ++ u32 v; + int data; + u32 value = MII_BUSY; + +@@ -86,12 +67,14 @@ static int stmmac_mdio_read(struct mii_b + if (priv->plat->has_gmac4) + value |= MII_GMAC4_READ; + +- if (stmmac_mdio_busy_wait(priv->ioaddr, mii_address)) ++ if (readl_poll_timeout(priv->ioaddr + mii_address, v, !(v & MII_BUSY), ++ 100, 10000)) + return -EBUSY; + + writel(value, priv->ioaddr + mii_address); + +- if (stmmac_mdio_busy_wait(priv->ioaddr, mii_address)) ++ if (readl_poll_timeout(priv->ioaddr + mii_address, v, !(v & MII_BUSY), ++ 100, 10000)) + return -EBUSY; + + /* Read the data from the MII data register */ +@@ -115,7 +98,7 @@ static int stmmac_mdio_write(struct mii_ + struct stmmac_priv *priv = netdev_priv(ndev); + unsigned int mii_address = priv->hw->mii.addr; + unsigned int mii_data = priv->hw->mii.data; +- ++ u32 v; + u32 value = MII_BUSY; + + value |= (phyaddr << priv->hw->mii.addr_shift) +@@ -130,7 +113,8 @@ static int stmmac_mdio_write(struct mii_ + value |= MII_WRITE; + + /* Wait until any existing MII operation is complete */ +- if (stmmac_mdio_busy_wait(priv->ioaddr, mii_address)) ++ if (readl_poll_timeout(priv->ioaddr + mii_address, v, !(v & MII_BUSY), ++ 100, 10000)) + return -EBUSY; + + /* Set the MII address register to write */ +@@ -138,7 +122,8 @@ static int stmmac_mdio_write(struct mii_ + writel(value, priv->ioaddr + mii_address); + + /* Wait until any existing MII operation is complete */ +- return stmmac_mdio_busy_wait(priv->ioaddr, mii_address); ++ return readl_poll_timeout(priv->ioaddr + mii_address, v, !(v & MII_BUSY), ++ 100, 10000); + } + + /** +@@ -156,9 +141,9 @@ int stmmac_mdio_reset(struct mii_bus *bu + + #ifdef CONFIG_OF + if (priv->device->of_node) { +- + if (data->reset_gpio < 0) { + struct device_node *np = priv->device->of_node; ++ + if (!np) + return 0; + +@@ -198,7 +183,7 @@ int stmmac_mdio_reset(struct mii_bus *bu + + /* This is a workaround for problems with the STE101P PHY. + * It doesn't complete its reset until at least one clock cycle +- * on MDC, so perform a dummy mdio read. To be upadted for GMAC4 ++ * on MDC, so perform a dummy mdio read. To be updated for GMAC4 + * if needed. + */ + if (!priv->plat->has_gmac4) +@@ -225,7 +210,7 @@ int stmmac_mdio_register(struct net_devi + return 0; + + new_bus = mdiobus_alloc(); +- if (new_bus == NULL) ++ if (!new_bus) + return -ENOMEM; + + if (mdio_bus_data->irqs) +@@ -262,49 +247,48 @@ int stmmac_mdio_register(struct net_devi + found = 0; + for (addr = 0; addr < PHY_MAX_ADDR; addr++) { + struct phy_device *phydev = mdiobus_get_phy(new_bus, addr); +- if (phydev) { +- int act = 0; +- char irq_num[4]; +- char *irq_str; +- +- /* +- * If an IRQ was provided to be assigned after +- * the bus probe, do it here. +- */ +- if ((mdio_bus_data->irqs == NULL) && +- (mdio_bus_data->probed_phy_irq > 0)) { +- new_bus->irq[addr] = +- mdio_bus_data->probed_phy_irq; +- phydev->irq = mdio_bus_data->probed_phy_irq; +- } +- +- /* +- * If we're going to bind the MAC to this PHY bus, +- * and no PHY number was provided to the MAC, +- * use the one probed here. +- */ +- if (priv->plat->phy_addr == -1) +- priv->plat->phy_addr = addr; +- +- act = (priv->plat->phy_addr == addr); +- switch (phydev->irq) { +- case PHY_POLL: +- irq_str = "POLL"; +- break; +- case PHY_IGNORE_INTERRUPT: +- irq_str = "IGNORE"; +- break; +- default: +- sprintf(irq_num, "%d", phydev->irq); +- irq_str = irq_num; +- break; +- } +- netdev_info(ndev, "PHY ID %08x at %d IRQ %s (%s)%s\n", +- phydev->phy_id, addr, +- irq_str, phydev_name(phydev), +- act ? " active" : ""); +- found = 1; ++ int act = 0; ++ char irq_num[4]; ++ char *irq_str; ++ ++ if (!phydev) ++ continue; ++ ++ /* ++ * If an IRQ was provided to be assigned after ++ * the bus probe, do it here. ++ */ ++ if (!mdio_bus_data->irqs && ++ (mdio_bus_data->probed_phy_irq > 0)) { ++ new_bus->irq[addr] = mdio_bus_data->probed_phy_irq; ++ phydev->irq = mdio_bus_data->probed_phy_irq; ++ } ++ ++ /* ++ * If we're going to bind the MAC to this PHY bus, ++ * and no PHY number was provided to the MAC, ++ * use the one probed here. ++ */ ++ if (priv->plat->phy_addr == -1) ++ priv->plat->phy_addr = addr; ++ ++ act = (priv->plat->phy_addr == addr); ++ switch (phydev->irq) { ++ case PHY_POLL: ++ irq_str = "POLL"; ++ break; ++ case PHY_IGNORE_INTERRUPT: ++ irq_str = "IGNORE"; ++ break; ++ default: ++ sprintf(irq_num, "%d", phydev->irq); ++ irq_str = irq_num; ++ break; + } ++ netdev_info(ndev, "PHY ID %08x at %d IRQ %s (%s)%s\n", ++ phydev->phy_id, addr, irq_str, phydev_name(phydev), ++ act ? " active" : ""); ++ found = 1; + } + + if (!found && !mdio_node) { +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +@@ -121,7 +117,6 @@ static struct stmmac_axi *stmmac_axi_set + axi->axi_lpi_en = of_property_read_bool(np, "snps,lpi_en"); + axi->axi_xit_frm = of_property_read_bool(np, "snps,xit_frm"); + axi->axi_kbbe = of_property_read_bool(np, "snps,axi_kbbe"); +- axi->axi_axi_all = of_property_read_bool(np, "snps,axi_all"); + axi->axi_fb = of_property_read_bool(np, "snps,axi_fb"); + axi->axi_mb = of_property_read_bool(np, "snps,axi_mb"); + axi->axi_rb = of_property_read_bool(np, "snps,axi_rb"); +@@ -181,10 +176,19 @@ static int stmmac_dt_phy(struct plat_stm + mdio = false; + } + +- /* If snps,dwmac-mdio is passed from DT, always register the MDIO */ +- for_each_child_of_node(np, plat->mdio_node) { +- if (of_device_is_compatible(plat->mdio_node, "snps,dwmac-mdio")) +- break; ++ /* exception for dwmac-dwc-qos-eth glue logic */ ++ if (of_device_is_compatible(np, "snps,dwc-qos-ethernet-4.10")) { ++ plat->mdio_node = of_get_child_by_name(np, "mdio"); ++ } else { ++ /** ++ * If snps,dwmac-mdio is passed from DT, always register ++ * the MDIO ++ */ ++ for_each_child_of_node(np, plat->mdio_node) { ++ if (of_device_is_compatible(plat->mdio_node, ++ "snps,dwmac-mdio")) ++ break; ++ } + } + + if (plat->mdio_node) { +@@ -249,6 +253,9 @@ stmmac_probe_config_dt(struct platform_d + plat->force_sf_dma_mode = + of_property_read_bool(np, "snps,force_sf_dma_mode"); + ++ plat->en_tx_lpi_clockgating = ++ of_property_read_bool(np, "snps,en-tx-lpi-clockgating"); ++ + /* Set the maxmtu to a default of JUMBO_LEN in case the + * parameter is not present in the device tree. + */ +@@ -333,7 +340,54 @@ stmmac_probe_config_dt(struct platform_d + + plat->axi = stmmac_axi_setup(pdev); + ++ /* clock setup */ ++ plat->stmmac_clk = devm_clk_get(&pdev->dev, ++ STMMAC_RESOURCE_NAME); ++ if (IS_ERR(plat->stmmac_clk)) { ++ dev_warn(&pdev->dev, "Cannot get CSR clock\n"); ++ plat->stmmac_clk = NULL; ++ } ++ clk_prepare_enable(plat->stmmac_clk); ++ ++ plat->pclk = devm_clk_get(&pdev->dev, "pclk"); ++ if (IS_ERR(plat->pclk)) { ++ if (PTR_ERR(plat->pclk) == -EPROBE_DEFER) ++ goto error_pclk_get; ++ ++ plat->pclk = NULL; ++ } ++ clk_prepare_enable(plat->pclk); ++ ++ /* Fall-back to main clock in case of no PTP ref is passed */ ++ plat->clk_ptp_ref = devm_clk_get(&pdev->dev, "clk_ptp_ref"); ++ if (IS_ERR(plat->clk_ptp_ref)) { ++ plat->clk_ptp_rate = clk_get_rate(plat->stmmac_clk); ++ plat->clk_ptp_ref = NULL; ++ dev_warn(&pdev->dev, "PTP uses main clock\n"); ++ } else { ++ clk_prepare_enable(plat->clk_ptp_ref); ++ plat->clk_ptp_rate = clk_get_rate(plat->clk_ptp_ref); ++ dev_dbg(&pdev->dev, "PTP rate %d\n", plat->clk_ptp_rate); ++ } ++ ++ plat->stmmac_rst = devm_reset_control_get(&pdev->dev, ++ STMMAC_RESOURCE_NAME); ++ if (IS_ERR(plat->stmmac_rst)) { ++ if (PTR_ERR(plat->stmmac_rst) == -EPROBE_DEFER) ++ goto error_hw_init; ++ ++ dev_info(&pdev->dev, "no reset control found\n"); ++ plat->stmmac_rst = NULL; ++ } ++ + return plat; ++ ++error_hw_init: ++ clk_disable_unprepare(plat->pclk); ++error_pclk_get: ++ clk_disable_unprepare(plat->stmmac_clk); ++ ++ return ERR_PTR(-EPROBE_DEFER); + } + + /** +@@ -357,7 +411,7 @@ void stmmac_remove_config_dt(struct plat + struct plat_stmmacenet_data * + stmmac_probe_config_dt(struct platform_device *pdev, const char **mac) + { +- return ERR_PTR(-ENOSYS); ++ return ERR_PTR(-EINVAL); + } + + void stmmac_remove_config_dt(struct platform_device *pdev, +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h +@@ -12,10 +12,6 @@ + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + +- You should have received a copy of the GNU General Public License along with +- this program; if not, write to the Free Software Foundation, Inc., +- 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. +- + The full GNU General Public License is included in this distribution in + the file called "COPYING". + +--- a/include/linux/stmmac.h ++++ b/include/linux/stmmac.h +@@ -103,7 +103,6 @@ struct stmmac_axi { + u32 axi_wr_osr_lmt; + u32 axi_rd_osr_lmt; + bool axi_kbbe; +- bool axi_axi_all; + u32 axi_blen[AXI_BLEN]; + bool axi_fb; + bool axi_mb; +@@ -135,13 +134,18 @@ struct plat_stmmacenet_data { + int tx_fifo_size; + int rx_fifo_size; + void (*fix_mac_speed)(void *priv, unsigned int speed); +- void (*bus_setup)(void __iomem *ioaddr); + int (*init)(struct platform_device *pdev, void *priv); + void (*exit)(struct platform_device *pdev, void *priv); + void *bsp_priv; ++ struct clk *stmmac_clk; ++ struct clk *pclk; ++ struct clk *clk_ptp_ref; ++ unsigned int clk_ptp_rate; ++ struct reset_control *stmmac_rst; + struct stmmac_axi *axi; + int has_gmac4; + bool tso_en; + int mac_port_sel_speed; ++ bool en_tx_lpi_clockgating; + }; + #endif diff --git a/target/linux/sunxi/patches-4.9/0052-stmmac-form-4-12.patch b/target/linux/sunxi/patches-4.9/0052-stmmac-form-4-12.patch new file mode 100644 index 0000000000..285e4d2762 --- /dev/null +++ b/target/linux/sunxi/patches-4.9/0052-stmmac-form-4-12.patch @@ -0,0 +1,5952 @@ +--- a/Documentation/devicetree/bindings/net/stmmac.txt ++++ b/Documentation/devicetree/bindings/net/stmmac.txt +@@ -7,9 +7,12 @@ Required properties: + - interrupt-parent: Should be the phandle for the interrupt controller + that services interrupts for this device + - interrupts: Should contain the STMMAC interrupts +-- interrupt-names: Should contain the interrupt names "macirq" +- "eth_wake_irq" if this interrupt is supported in the "interrupts" +- property ++- interrupt-names: Should contain a list of interrupt names corresponding to ++ the interrupts in the interrupts property, if available. ++ Valid interrupt names are: ++ - "macirq" (combined signal for various interrupt events) ++ - "eth_wake_irq" (the interrupt to manage the remote wake-up packet detection) ++ - "eth_lpi" (the interrupt that occurs when Tx or Rx enters/exits LPI state) + - phy-mode: See ethernet.txt file in the same directory. + - snps,reset-gpio gpio number for phy reset. + - snps,reset-active-low boolean flag to indicate if phy reset is active low. +@@ -28,9 +31,9 @@ Optional properties: + clocks may be specified in derived bindings. + - clock-names: One name for each entry in the clocks property, the + first one should be "stmmaceth" and the second one should be "pclk". +-- clk_ptp_ref: this is the PTP reference clock; in case of the PTP is +- available this clock is used for programming the Timestamp Addend Register. +- If not passed then the system clock will be used and this is fine on some ++- ptp_ref: this is the PTP reference clock; in case of the PTP is available ++ this clock is used for programming the Timestamp Addend Register. If not ++ passed then the system clock will be used and this is fine on some + platforms. + - tx-fifo-depth: See ethernet.txt file in the same directory + - rx-fifo-depth: See ethernet.txt file in the same directory +@@ -72,7 +75,45 @@ Optional properties: + - snps,mb: mixed-burst + - snps,rb: rebuild INCRx Burst + - mdio: with compatible = "snps,dwmac-mdio", create and register mdio bus. +- ++- Multiple RX Queues parameters: below the list of all the parameters to ++ configure the multiple RX queues: ++ - snps,rx-queues-to-use: number of RX queues to be used in the driver ++ - Choose one of these RX scheduling algorithms: ++ - snps,rx-sched-sp: Strict priority ++ - snps,rx-sched-wsp: Weighted Strict priority ++ - For each RX queue ++ - Choose one of these modes: ++ - snps,dcb-algorithm: Queue to be enabled as DCB ++ - snps,avb-algorithm: Queue to be enabled as AVB ++ - snps,map-to-dma-channel: Channel to map ++ - Specifiy specific packet routing: ++ - snps,route-avcp: AV Untagged Control packets ++ - snps,route-ptp: PTP Packets ++ - snps,route-dcbcp: DCB Control Packets ++ - snps,route-up: Untagged Packets ++ - snps,route-multi-broad: Multicast & Broadcast Packets ++ - snps,priority: RX queue priority (Range: 0x0 to 0xF) ++- Multiple TX Queues parameters: below the list of all the parameters to ++ configure the multiple TX queues: ++ - snps,tx-queues-to-use: number of TX queues to be used in the driver ++ - Choose one of these TX scheduling algorithms: ++ - snps,tx-sched-wrr: Weighted Round Robin ++ - snps,tx-sched-wfq: Weighted Fair Queuing ++ - snps,tx-sched-dwrr: Deficit Weighted Round Robin ++ - snps,tx-sched-sp: Strict priority ++ - For each TX queue ++ - snps,weight: TX queue weight (if using a DCB weight algorithm) ++ - Choose one of these modes: ++ - snps,dcb-algorithm: TX queue will be working in DCB ++ - snps,avb-algorithm: TX queue will be working in AVB ++ [Attention] Queue 0 is reserved for legacy traffic ++ and so no AVB is available in this queue. ++ - Configure Credit Base Shaper (if AVB Mode selected): ++ - snps,send_slope: enable Low Power Interface ++ - snps,idle_slope: unlock on WoL ++ - snps,high_credit: max write outstanding req. limit ++ - snps,low_credit: max read outstanding req. limit ++ - snps,priority: TX queue priority (Range: 0x0 to 0xF) + Examples: + + stmmac_axi_setup: stmmac-axi-config { +@@ -81,12 +122,41 @@ Examples: + snps,blen = <256 128 64 32 0 0 0>; + }; + ++ mtl_rx_setup: rx-queues-config { ++ snps,rx-queues-to-use = <1>; ++ snps,rx-sched-sp; ++ queue0 { ++ snps,dcb-algorithm; ++ snps,map-to-dma-channel = <0x0>; ++ snps,priority = <0x0>; ++ }; ++ }; ++ ++ mtl_tx_setup: tx-queues-config { ++ snps,tx-queues-to-use = <2>; ++ snps,tx-sched-wrr; ++ queue0 { ++ snps,weight = <0x10>; ++ snps,dcb-algorithm; ++ snps,priority = <0x0>; ++ }; ++ ++ queue1 { ++ snps,avb-algorithm; ++ snps,send_slope = <0x1000>; ++ snps,idle_slope = <0x1000>; ++ snps,high_credit = <0x3E800>; ++ snps,low_credit = <0xFFC18000>; ++ snps,priority = <0x1>; ++ }; ++ }; ++ + gmac0: ethernet@e0800000 { + compatible = "st,spear600-gmac"; + reg = <0xe0800000 0x8000>; + interrupt-parent = <&vic1>; +- interrupts = <24 23>; +- interrupt-names = "macirq", "eth_wake_irq"; ++ interrupts = <24 23 22>; ++ interrupt-names = "macirq", "eth_wake_irq", "eth_lpi"; + mac-address = [000000000000]; /* Filled in by U-Boot */ + max-frame-size = <3800>; + phy-mode = "gmii"; +@@ -104,4 +174,6 @@ Examples: + phy1: ethernet-phy@0 { + }; + }; ++ snps,mtl-rx-config = <&mtl_rx_setup>; ++ snps,mtl-tx-config = <&mtl_tx_setup>; + }; +--- a/drivers/net/ethernet/stmicro/stmmac/altr_tse_pcs.c ++++ b/drivers/net/ethernet/stmicro/stmmac/altr_tse_pcs.c +@@ -37,6 +37,7 @@ + #define TSE_PCS_CONTROL_AN_EN_MASK BIT(12) + #define TSE_PCS_CONTROL_REG 0x00 + #define TSE_PCS_CONTROL_RESTART_AN_MASK BIT(9) ++#define TSE_PCS_CTRL_AUTONEG_SGMII 0x1140 + #define TSE_PCS_IF_MODE_REG 0x28 + #define TSE_PCS_LINK_TIMER_0_REG 0x24 + #define TSE_PCS_LINK_TIMER_1_REG 0x26 +@@ -65,6 +66,7 @@ + #define TSE_PCS_SW_RESET_TIMEOUT 100 + #define TSE_PCS_USE_SGMII_AN_MASK BIT(1) + #define TSE_PCS_USE_SGMII_ENA BIT(0) ++#define TSE_PCS_IF_USE_SGMII 0x03 + + #define SGMII_ADAPTER_CTRL_REG 0x00 + #define SGMII_ADAPTER_DISABLE 0x0001 +@@ -101,7 +103,9 @@ int tse_pcs_init(void __iomem *base, str + { + int ret = 0; + +- writew(TSE_PCS_USE_SGMII_ENA, base + TSE_PCS_IF_MODE_REG); ++ writew(TSE_PCS_IF_USE_SGMII, base + TSE_PCS_IF_MODE_REG); ++ ++ writew(TSE_PCS_CTRL_AUTONEG_SGMII, base + TSE_PCS_CONTROL_REG); + + writew(TSE_PCS_SGMII_LINK_TIMER_0, base + TSE_PCS_LINK_TIMER_0_REG); + writew(TSE_PCS_SGMII_LINK_TIMER_1, base + TSE_PCS_LINK_TIMER_1_REG); +--- a/drivers/net/ethernet/stmicro/stmmac/chain_mode.c ++++ b/drivers/net/ethernet/stmicro/stmmac/chain_mode.c +@@ -26,12 +26,15 @@ + + static int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum) + { +- struct stmmac_priv *priv = (struct stmmac_priv *)p; +- unsigned int entry = priv->cur_tx; +- struct dma_desc *desc = priv->dma_tx + entry; ++ struct stmmac_tx_queue *tx_q = (struct stmmac_tx_queue *)p; + unsigned int nopaged_len = skb_headlen(skb); ++ struct stmmac_priv *priv = tx_q->priv_data; ++ unsigned int entry = tx_q->cur_tx; + unsigned int bmax, des2; + unsigned int i = 1, len; ++ struct dma_desc *desc; ++ ++ desc = tx_q->dma_tx + entry; + + if (priv->plat->enh_desc) + bmax = BUF_SIZE_8KiB; +@@ -45,16 +48,16 @@ static int stmmac_jumbo_frm(void *p, str + desc->des2 = cpu_to_le32(des2); + if (dma_mapping_error(priv->device, des2)) + return -1; +- priv->tx_skbuff_dma[entry].buf = des2; +- priv->tx_skbuff_dma[entry].len = bmax; ++ tx_q->tx_skbuff_dma[entry].buf = des2; ++ tx_q->tx_skbuff_dma[entry].len = bmax; + /* do not close the descriptor and do not set own bit */ + priv->hw->desc->prepare_tx_desc(desc, 1, bmax, csum, STMMAC_CHAIN_MODE, +- 0, false); ++ 0, false, skb->len); + + while (len != 0) { +- priv->tx_skbuff[entry] = NULL; ++ tx_q->tx_skbuff[entry] = NULL; + entry = STMMAC_GET_ENTRY(entry, DMA_TX_SIZE); +- desc = priv->dma_tx + entry; ++ desc = tx_q->dma_tx + entry; + + if (len > bmax) { + des2 = dma_map_single(priv->device, +@@ -63,11 +66,11 @@ static int stmmac_jumbo_frm(void *p, str + desc->des2 = cpu_to_le32(des2); + if (dma_mapping_error(priv->device, des2)) + return -1; +- priv->tx_skbuff_dma[entry].buf = des2; +- priv->tx_skbuff_dma[entry].len = bmax; ++ tx_q->tx_skbuff_dma[entry].buf = des2; ++ tx_q->tx_skbuff_dma[entry].len = bmax; + priv->hw->desc->prepare_tx_desc(desc, 0, bmax, csum, + STMMAC_CHAIN_MODE, 1, +- false); ++ false, skb->len); + len -= bmax; + i++; + } else { +@@ -77,17 +80,17 @@ static int stmmac_jumbo_frm(void *p, str + desc->des2 = cpu_to_le32(des2); + if (dma_mapping_error(priv->device, des2)) + return -1; +- priv->tx_skbuff_dma[entry].buf = des2; +- priv->tx_skbuff_dma[entry].len = len; ++ tx_q->tx_skbuff_dma[entry].buf = des2; ++ tx_q->tx_skbuff_dma[entry].len = len; + /* last descriptor can be set now */ + priv->hw->desc->prepare_tx_desc(desc, 0, len, csum, + STMMAC_CHAIN_MODE, 1, +- true); ++ true, skb->len); + len = 0; + } + } + +- priv->cur_tx = entry; ++ tx_q->cur_tx = entry; + + return entry; + } +@@ -136,32 +139,34 @@ static void stmmac_init_dma_chain(void * + + static void stmmac_refill_desc3(void *priv_ptr, struct dma_desc *p) + { +- struct stmmac_priv *priv = (struct stmmac_priv *)priv_ptr; ++ struct stmmac_rx_queue *rx_q = (struct stmmac_rx_queue *)priv_ptr; ++ struct stmmac_priv *priv = rx_q->priv_data; + + if (priv->hwts_rx_en && !priv->extend_desc) + /* NOTE: Device will overwrite des3 with timestamp value if + * 1588-2002 time stamping is enabled, hence reinitialize it + * to keep explicit chaining in the descriptor. + */ +- p->des3 = cpu_to_le32((unsigned int)(priv->dma_rx_phy + +- (((priv->dirty_rx) + 1) % ++ p->des3 = cpu_to_le32((unsigned int)(rx_q->dma_rx_phy + ++ (((rx_q->dirty_rx) + 1) % + DMA_RX_SIZE) * + sizeof(struct dma_desc))); + } + + static void stmmac_clean_desc3(void *priv_ptr, struct dma_desc *p) + { +- struct stmmac_priv *priv = (struct stmmac_priv *)priv_ptr; +- unsigned int entry = priv->dirty_tx; ++ struct stmmac_tx_queue *tx_q = (struct stmmac_tx_queue *)priv_ptr; ++ struct stmmac_priv *priv = tx_q->priv_data; ++ unsigned int entry = tx_q->dirty_tx; + +- if (priv->tx_skbuff_dma[entry].last_segment && !priv->extend_desc && ++ if (tx_q->tx_skbuff_dma[entry].last_segment && !priv->extend_desc && + priv->hwts_tx_en) + /* NOTE: Device will overwrite des3 with timestamp value if + * 1588-2002 time stamping is enabled, hence reinitialize it + * to keep explicit chaining in the descriptor. + */ +- p->des3 = cpu_to_le32((unsigned int)((priv->dma_tx_phy + +- ((priv->dirty_tx + 1) % DMA_TX_SIZE)) ++ p->des3 = cpu_to_le32((unsigned int)((tx_q->dma_tx_phy + ++ ((tx_q->dirty_tx + 1) % DMA_TX_SIZE)) + * sizeof(struct dma_desc))); + } + +--- a/drivers/net/ethernet/stmicro/stmmac/common.h ++++ b/drivers/net/ethernet/stmicro/stmmac/common.h +@@ -246,6 +246,15 @@ struct stmmac_extra_stats { + #define STMMAC_TX_MAX_FRAMES 256 + #define STMMAC_TX_FRAMES 64 + ++/* Packets types */ ++enum packets_types { ++ PACKET_AVCPQ = 0x1, /* AV Untagged Control packets */ ++ PACKET_PTPQ = 0x2, /* PTP Packets */ ++ PACKET_DCBCPQ = 0x3, /* DCB Control Packets */ ++ PACKET_UPQ = 0x4, /* Untagged Packets */ ++ PACKET_MCBCQ = 0x5, /* Multicast & Broadcast Packets */ ++}; ++ + /* Rx IPC status */ + enum rx_frame_status { + good_frame = 0x0, +@@ -324,6 +333,9 @@ struct dma_features { + unsigned int number_tx_queues; + /* Alternate (enhanced) DESC mode */ + unsigned int enh_desc; ++ /* TX and RX FIFO sizes */ ++ unsigned int tx_fifo_size; ++ unsigned int rx_fifo_size; + }; + + /* GMAC TX FIFO is 8K, Rx FIFO is 16K */ +@@ -361,7 +373,7 @@ struct stmmac_desc_ops { + /* Invoked by the xmit function to prepare the tx descriptor */ + void (*prepare_tx_desc) (struct dma_desc *p, int is_fs, int len, + bool csum_flag, int mode, bool tx_own, +- bool ls); ++ bool ls, unsigned int tot_pkt_len); + void (*prepare_tso_tx_desc)(struct dma_desc *p, int is_fs, int len1, + int len2, bool tx_own, bool ls, + unsigned int tcphdrlen, +@@ -413,6 +425,14 @@ struct stmmac_dma_ops { + int (*reset)(void __iomem *ioaddr); + void (*init)(void __iomem *ioaddr, struct stmmac_dma_cfg *dma_cfg, + u32 dma_tx, u32 dma_rx, int atds); ++ void (*init_chan)(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, u32 chan); ++ void (*init_rx_chan)(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, ++ u32 dma_rx_phy, u32 chan); ++ void (*init_tx_chan)(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, ++ u32 dma_tx_phy, u32 chan); + /* Configure the AXI Bus Mode Register */ + void (*axi)(void __iomem *ioaddr, struct stmmac_axi *axi); + /* Dump DMA registers */ +@@ -421,25 +441,28 @@ struct stmmac_dma_ops { + * An invalid value enables the store-and-forward mode */ + void (*dma_mode)(void __iomem *ioaddr, int txmode, int rxmode, + int rxfifosz); ++ void (*dma_rx_mode)(void __iomem *ioaddr, int mode, u32 channel, ++ int fifosz); ++ void (*dma_tx_mode)(void __iomem *ioaddr, int mode, u32 channel); + /* To track extra statistic (if supported) */ + void (*dma_diagnostic_fr) (void *data, struct stmmac_extra_stats *x, + void __iomem *ioaddr); + void (*enable_dma_transmission) (void __iomem *ioaddr); +- void (*enable_dma_irq) (void __iomem *ioaddr); +- void (*disable_dma_irq) (void __iomem *ioaddr); +- void (*start_tx) (void __iomem *ioaddr); +- void (*stop_tx) (void __iomem *ioaddr); +- void (*start_rx) (void __iomem *ioaddr); +- void (*stop_rx) (void __iomem *ioaddr); ++ void (*enable_dma_irq)(void __iomem *ioaddr, u32 chan); ++ void (*disable_dma_irq)(void __iomem *ioaddr, u32 chan); ++ void (*start_tx)(void __iomem *ioaddr, u32 chan); ++ void (*stop_tx)(void __iomem *ioaddr, u32 chan); ++ void (*start_rx)(void __iomem *ioaddr, u32 chan); ++ void (*stop_rx)(void __iomem *ioaddr, u32 chan); + int (*dma_interrupt) (void __iomem *ioaddr, +- struct stmmac_extra_stats *x); ++ struct stmmac_extra_stats *x, u32 chan); + /* If supported then get the optional core features */ + void (*get_hw_feature)(void __iomem *ioaddr, + struct dma_features *dma_cap); + /* Program the HW RX Watchdog */ +- void (*rx_watchdog) (void __iomem *ioaddr, u32 riwt); +- void (*set_tx_ring_len)(void __iomem *ioaddr, u32 len); +- void (*set_rx_ring_len)(void __iomem *ioaddr, u32 len); ++ void (*rx_watchdog)(void __iomem *ioaddr, u32 riwt, u32 number_chan); ++ void (*set_tx_ring_len)(void __iomem *ioaddr, u32 len, u32 chan); ++ void (*set_rx_ring_len)(void __iomem *ioaddr, u32 len, u32 chan); + void (*set_rx_tail_ptr)(void __iomem *ioaddr, u32 tail_ptr, u32 chan); + void (*set_tx_tail_ptr)(void __iomem *ioaddr, u32 tail_ptr, u32 chan); + void (*enable_tso)(void __iomem *ioaddr, bool en, u32 chan); +@@ -451,20 +474,44 @@ struct mac_device_info; + struct stmmac_ops { + /* MAC core initialization */ + void (*core_init)(struct mac_device_info *hw, int mtu); ++ /* Enable the MAC RX/TX */ ++ void (*set_mac)(void __iomem *ioaddr, bool enable); + /* Enable and verify that the IPC module is supported */ + int (*rx_ipc)(struct mac_device_info *hw); + /* Enable RX Queues */ +- void (*rx_queue_enable)(struct mac_device_info *hw, u32 queue); ++ void (*rx_queue_enable)(struct mac_device_info *hw, u8 mode, u32 queue); ++ /* RX Queues Priority */ ++ void (*rx_queue_prio)(struct mac_device_info *hw, u32 prio, u32 queue); ++ /* TX Queues Priority */ ++ void (*tx_queue_prio)(struct mac_device_info *hw, u32 prio, u32 queue); ++ /* RX Queues Routing */ ++ void (*rx_queue_routing)(struct mac_device_info *hw, u8 packet, ++ u32 queue); ++ /* Program RX Algorithms */ ++ void (*prog_mtl_rx_algorithms)(struct mac_device_info *hw, u32 rx_alg); ++ /* Program TX Algorithms */ ++ void (*prog_mtl_tx_algorithms)(struct mac_device_info *hw, u32 tx_alg); ++ /* Set MTL TX queues weight */ ++ void (*set_mtl_tx_queue_weight)(struct mac_device_info *hw, ++ u32 weight, u32 queue); ++ /* RX MTL queue to RX dma mapping */ ++ void (*map_mtl_to_dma)(struct mac_device_info *hw, u32 queue, u32 chan); ++ /* Configure AV Algorithm */ ++ void (*config_cbs)(struct mac_device_info *hw, u32 send_slope, ++ u32 idle_slope, u32 high_credit, u32 low_credit, ++ u32 queue); + /* Dump MAC registers */ + void (*dump_regs)(struct mac_device_info *hw, u32 *reg_space); + /* Handle extra events on specific interrupts hw dependent */ + int (*host_irq_status)(struct mac_device_info *hw, + struct stmmac_extra_stats *x); ++ /* Handle MTL interrupts */ ++ int (*host_mtl_irq_status)(struct mac_device_info *hw, u32 chan); + /* Multicast filter setting */ + void (*set_filter)(struct mac_device_info *hw, struct net_device *dev); + /* Flow control setting */ + void (*flow_ctrl)(struct mac_device_info *hw, unsigned int duplex, +- unsigned int fc, unsigned int pause_time); ++ unsigned int fc, unsigned int pause_time, u32 tx_cnt); + /* Set power management mode (e.g. magic frame) */ + void (*pmt)(struct mac_device_info *hw, unsigned long mode); + /* Set/Get Unicast MAC addresses */ +@@ -477,7 +524,8 @@ struct stmmac_ops { + void (*reset_eee_mode)(struct mac_device_info *hw); + void (*set_eee_timer)(struct mac_device_info *hw, int ls, int tw); + void (*set_eee_pls)(struct mac_device_info *hw, int link); +- void (*debug)(void __iomem *ioaddr, struct stmmac_extra_stats *x); ++ void (*debug)(void __iomem *ioaddr, struct stmmac_extra_stats *x, ++ u32 rx_queues, u32 tx_queues); + /* PCS calls */ + void (*pcs_ctrl_ane)(void __iomem *ioaddr, bool ane, bool srgmi_ral, + bool loopback); +@@ -547,6 +595,11 @@ struct mac_device_info { + unsigned int ps; + }; + ++struct stmmac_rx_routing { ++ u32 reg_mask; ++ u32 reg_shift; ++}; ++ + struct mac_device_info *dwmac1000_setup(void __iomem *ioaddr, int mcbins, + int perfect_uc_entries, + int *synopsys_id); +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-dwc-qos-eth.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-dwc-qos-eth.c +@@ -14,16 +14,34 @@ + #include + #include + #include ++#include + #include + #include ++#include + #include + #include ++#include + #include + #include + #include ++#include + #include + + #include "stmmac_platform.h" ++#include "dwmac4.h" ++ ++struct tegra_eqos { ++ struct device *dev; ++ void __iomem *regs; ++ ++ struct reset_control *rst; ++ struct clk *clk_master; ++ struct clk *clk_slave; ++ struct clk *clk_tx; ++ struct clk *clk_rx; ++ ++ struct gpio_desc *reset; ++}; + + static int dwc_eth_dwmac_config_dt(struct platform_device *pdev, + struct plat_stmmacenet_data *plat_dat) +@@ -106,13 +124,309 @@ static int dwc_eth_dwmac_config_dt(struc + return 0; + } + ++static void *dwc_qos_probe(struct platform_device *pdev, ++ struct plat_stmmacenet_data *plat_dat, ++ struct stmmac_resources *stmmac_res) ++{ ++ int err; ++ ++ plat_dat->stmmac_clk = devm_clk_get(&pdev->dev, "apb_pclk"); ++ if (IS_ERR(plat_dat->stmmac_clk)) { ++ dev_err(&pdev->dev, "apb_pclk clock not found.\n"); ++ return ERR_CAST(plat_dat->stmmac_clk); ++ } ++ ++ err = clk_prepare_enable(plat_dat->stmmac_clk); ++ if (err < 0) { ++ dev_err(&pdev->dev, "failed to enable apb_pclk clock: %d\n", ++ err); ++ return ERR_PTR(err); ++ } ++ ++ plat_dat->pclk = devm_clk_get(&pdev->dev, "phy_ref_clk"); ++ if (IS_ERR(plat_dat->pclk)) { ++ dev_err(&pdev->dev, "phy_ref_clk clock not found.\n"); ++ err = PTR_ERR(plat_dat->pclk); ++ goto disable; ++ } ++ ++ err = clk_prepare_enable(plat_dat->pclk); ++ if (err < 0) { ++ dev_err(&pdev->dev, "failed to enable phy_ref clock: %d\n", ++ err); ++ goto disable; ++ } ++ ++ return NULL; ++ ++disable: ++ clk_disable_unprepare(plat_dat->stmmac_clk); ++ return ERR_PTR(err); ++} ++ ++static int dwc_qos_remove(struct platform_device *pdev) ++{ ++ struct net_device *ndev = platform_get_drvdata(pdev); ++ struct stmmac_priv *priv = netdev_priv(ndev); ++ ++ clk_disable_unprepare(priv->plat->pclk); ++ clk_disable_unprepare(priv->plat->stmmac_clk); ++ ++ return 0; ++} ++ ++#define SDMEMCOMPPADCTRL 0x8800 ++#define SDMEMCOMPPADCTRL_PAD_E_INPUT_OR_E_PWRD BIT(31) ++ ++#define AUTO_CAL_CONFIG 0x8804 ++#define AUTO_CAL_CONFIG_START BIT(31) ++#define AUTO_CAL_CONFIG_ENABLE BIT(29) ++ ++#define AUTO_CAL_STATUS 0x880c ++#define AUTO_CAL_STATUS_ACTIVE BIT(31) ++ ++static void tegra_eqos_fix_speed(void *priv, unsigned int speed) ++{ ++ struct tegra_eqos *eqos = priv; ++ unsigned long rate = 125000000; ++ bool needs_calibration = false; ++ u32 value; ++ int err; ++ ++ switch (speed) { ++ case SPEED_1000: ++ needs_calibration = true; ++ rate = 125000000; ++ break; ++ ++ case SPEED_100: ++ needs_calibration = true; ++ rate = 25000000; ++ break; ++ ++ case SPEED_10: ++ rate = 2500000; ++ break; ++ ++ default: ++ dev_err(eqos->dev, "invalid speed %u\n", speed); ++ break; ++ } ++ ++ if (needs_calibration) { ++ /* calibrate */ ++ value = readl(eqos->regs + SDMEMCOMPPADCTRL); ++ value |= SDMEMCOMPPADCTRL_PAD_E_INPUT_OR_E_PWRD; ++ writel(value, eqos->regs + SDMEMCOMPPADCTRL); ++ ++ udelay(1); ++ ++ value = readl(eqos->regs + AUTO_CAL_CONFIG); ++ value |= AUTO_CAL_CONFIG_START | AUTO_CAL_CONFIG_ENABLE; ++ writel(value, eqos->regs + AUTO_CAL_CONFIG); ++ ++ err = readl_poll_timeout_atomic(eqos->regs + AUTO_CAL_STATUS, ++ value, ++ value & AUTO_CAL_STATUS_ACTIVE, ++ 1, 10); ++ if (err < 0) { ++ dev_err(eqos->dev, "calibration did not start\n"); ++ goto failed; ++ } ++ ++ err = readl_poll_timeout_atomic(eqos->regs + AUTO_CAL_STATUS, ++ value, ++ (value & AUTO_CAL_STATUS_ACTIVE) == 0, ++ 20, 200); ++ if (err < 0) { ++ dev_err(eqos->dev, "calibration didn't finish\n"); ++ goto failed; ++ } ++ ++ failed: ++ value = readl(eqos->regs + SDMEMCOMPPADCTRL); ++ value &= ~SDMEMCOMPPADCTRL_PAD_E_INPUT_OR_E_PWRD; ++ writel(value, eqos->regs + SDMEMCOMPPADCTRL); ++ } else { ++ value = readl(eqos->regs + AUTO_CAL_CONFIG); ++ value &= ~AUTO_CAL_CONFIG_ENABLE; ++ writel(value, eqos->regs + AUTO_CAL_CONFIG); ++ } ++ ++ err = clk_set_rate(eqos->clk_tx, rate); ++ if (err < 0) ++ dev_err(eqos->dev, "failed to set TX rate: %d\n", err); ++} ++ ++static int tegra_eqos_init(struct platform_device *pdev, void *priv) ++{ ++ struct tegra_eqos *eqos = priv; ++ unsigned long rate; ++ u32 value; ++ ++ rate = clk_get_rate(eqos->clk_slave); ++ ++ value = (rate / 1000000) - 1; ++ writel(value, eqos->regs + GMAC_1US_TIC_COUNTER); ++ ++ return 0; ++} ++ ++static void *tegra_eqos_probe(struct platform_device *pdev, ++ struct plat_stmmacenet_data *data, ++ struct stmmac_resources *res) ++{ ++ struct tegra_eqos *eqos; ++ int err; ++ ++ eqos = devm_kzalloc(&pdev->dev, sizeof(*eqos), GFP_KERNEL); ++ if (!eqos) { ++ err = -ENOMEM; ++ goto error; ++ } ++ ++ eqos->dev = &pdev->dev; ++ eqos->regs = res->addr; ++ ++ eqos->clk_master = devm_clk_get(&pdev->dev, "master_bus"); ++ if (IS_ERR(eqos->clk_master)) { ++ err = PTR_ERR(eqos->clk_master); ++ goto error; ++ } ++ ++ err = clk_prepare_enable(eqos->clk_master); ++ if (err < 0) ++ goto error; ++ ++ eqos->clk_slave = devm_clk_get(&pdev->dev, "slave_bus"); ++ if (IS_ERR(eqos->clk_slave)) { ++ err = PTR_ERR(eqos->clk_slave); ++ goto disable_master; ++ } ++ ++ data->stmmac_clk = eqos->clk_slave; ++ ++ err = clk_prepare_enable(eqos->clk_slave); ++ if (err < 0) ++ goto disable_master; ++ ++ eqos->clk_rx = devm_clk_get(&pdev->dev, "rx"); ++ if (IS_ERR(eqos->clk_rx)) { ++ err = PTR_ERR(eqos->clk_rx); ++ goto disable_slave; ++ } ++ ++ err = clk_prepare_enable(eqos->clk_rx); ++ if (err < 0) ++ goto disable_slave; ++ ++ eqos->clk_tx = devm_clk_get(&pdev->dev, "tx"); ++ if (IS_ERR(eqos->clk_tx)) { ++ err = PTR_ERR(eqos->clk_tx); ++ goto disable_rx; ++ } ++ ++ err = clk_prepare_enable(eqos->clk_tx); ++ if (err < 0) ++ goto disable_rx; ++ ++ eqos->reset = devm_gpiod_get(&pdev->dev, "phy-reset", GPIOD_OUT_HIGH); ++ if (IS_ERR(eqos->reset)) { ++ err = PTR_ERR(eqos->reset); ++ goto disable_tx; ++ } ++ ++ usleep_range(2000, 4000); ++ gpiod_set_value(eqos->reset, 0); ++ ++ eqos->rst = devm_reset_control_get(&pdev->dev, "eqos"); ++ if (IS_ERR(eqos->rst)) { ++ err = PTR_ERR(eqos->rst); ++ goto reset_phy; ++ } ++ ++ err = reset_control_assert(eqos->rst); ++ if (err < 0) ++ goto reset_phy; ++ ++ usleep_range(2000, 4000); ++ ++ err = reset_control_deassert(eqos->rst); ++ if (err < 0) ++ goto reset_phy; ++ ++ usleep_range(2000, 4000); ++ ++ data->fix_mac_speed = tegra_eqos_fix_speed; ++ data->init = tegra_eqos_init; ++ data->bsp_priv = eqos; ++ ++ err = tegra_eqos_init(pdev, eqos); ++ if (err < 0) ++ goto reset; ++ ++out: ++ return eqos; ++ ++reset: ++ reset_control_assert(eqos->rst); ++reset_phy: ++ gpiod_set_value(eqos->reset, 1); ++disable_tx: ++ clk_disable_unprepare(eqos->clk_tx); ++disable_rx: ++ clk_disable_unprepare(eqos->clk_rx); ++disable_slave: ++ clk_disable_unprepare(eqos->clk_slave); ++disable_master: ++ clk_disable_unprepare(eqos->clk_master); ++error: ++ eqos = ERR_PTR(err); ++ goto out; ++} ++ ++static int tegra_eqos_remove(struct platform_device *pdev) ++{ ++ struct tegra_eqos *eqos = get_stmmac_bsp_priv(&pdev->dev); ++ ++ reset_control_assert(eqos->rst); ++ gpiod_set_value(eqos->reset, 1); ++ clk_disable_unprepare(eqos->clk_tx); ++ clk_disable_unprepare(eqos->clk_rx); ++ clk_disable_unprepare(eqos->clk_slave); ++ clk_disable_unprepare(eqos->clk_master); ++ ++ return 0; ++} ++ ++struct dwc_eth_dwmac_data { ++ void *(*probe)(struct platform_device *pdev, ++ struct plat_stmmacenet_data *data, ++ struct stmmac_resources *res); ++ int (*remove)(struct platform_device *pdev); ++}; ++ ++static const struct dwc_eth_dwmac_data dwc_qos_data = { ++ .probe = dwc_qos_probe, ++ .remove = dwc_qos_remove, ++}; ++ ++static const struct dwc_eth_dwmac_data tegra_eqos_data = { ++ .probe = tegra_eqos_probe, ++ .remove = tegra_eqos_remove, ++}; ++ + static int dwc_eth_dwmac_probe(struct platform_device *pdev) + { ++ const struct dwc_eth_dwmac_data *data; + struct plat_stmmacenet_data *plat_dat; + struct stmmac_resources stmmac_res; + struct resource *res; ++ void *priv; + int ret; + ++ data = of_device_get_match_data(&pdev->dev); ++ + memset(&stmmac_res, 0, sizeof(struct stmmac_resources)); + + /** +@@ -138,39 +452,26 @@ static int dwc_eth_dwmac_probe(struct pl + if (IS_ERR(plat_dat)) + return PTR_ERR(plat_dat); + +- plat_dat->stmmac_clk = devm_clk_get(&pdev->dev, "apb_pclk"); +- if (IS_ERR(plat_dat->stmmac_clk)) { +- dev_err(&pdev->dev, "apb_pclk clock not found.\n"); +- ret = PTR_ERR(plat_dat->stmmac_clk); +- plat_dat->stmmac_clk = NULL; +- goto err_remove_config_dt; ++ priv = data->probe(pdev, plat_dat, &stmmac_res); ++ if (IS_ERR(priv)) { ++ ret = PTR_ERR(priv); ++ dev_err(&pdev->dev, "failed to probe subdriver: %d\n", ret); ++ goto remove_config; + } +- clk_prepare_enable(plat_dat->stmmac_clk); +- +- plat_dat->pclk = devm_clk_get(&pdev->dev, "phy_ref_clk"); +- if (IS_ERR(plat_dat->pclk)) { +- dev_err(&pdev->dev, "phy_ref_clk clock not found.\n"); +- ret = PTR_ERR(plat_dat->pclk); +- plat_dat->pclk = NULL; +- goto err_out_clk_dis_phy; +- } +- clk_prepare_enable(plat_dat->pclk); + + ret = dwc_eth_dwmac_config_dt(pdev, plat_dat); + if (ret) +- goto err_out_clk_dis_aper; ++ goto remove; + + ret = stmmac_dvr_probe(&pdev->dev, plat_dat, &stmmac_res); + if (ret) +- goto err_out_clk_dis_aper; ++ goto remove; + +- return 0; ++ return ret; + +-err_out_clk_dis_aper: +- clk_disable_unprepare(plat_dat->pclk); +-err_out_clk_dis_phy: +- clk_disable_unprepare(plat_dat->stmmac_clk); +-err_remove_config_dt: ++remove: ++ data->remove(pdev); ++remove_config: + stmmac_remove_config_dt(pdev, plat_dat); + + return ret; +@@ -178,11 +479,29 @@ err_remove_config_dt: + + static int dwc_eth_dwmac_remove(struct platform_device *pdev) + { +- return stmmac_pltfr_remove(pdev); ++ struct net_device *ndev = platform_get_drvdata(pdev); ++ struct stmmac_priv *priv = netdev_priv(ndev); ++ const struct dwc_eth_dwmac_data *data; ++ int err; ++ ++ data = of_device_get_match_data(&pdev->dev); ++ ++ err = stmmac_dvr_remove(&pdev->dev); ++ if (err < 0) ++ dev_err(&pdev->dev, "failed to remove platform: %d\n", err); ++ ++ err = data->remove(pdev); ++ if (err < 0) ++ dev_err(&pdev->dev, "failed to remove subdriver: %d\n", err); ++ ++ stmmac_remove_config_dt(pdev, priv->plat); ++ ++ return err; + } + + static const struct of_device_id dwc_eth_dwmac_match[] = { +- { .compatible = "snps,dwc-qos-ethernet-4.10", }, ++ { .compatible = "snps,dwc-qos-ethernet-4.10", .data = &dwc_qos_data }, ++ { .compatible = "nvidia,tegra186-eqos", .data = &tegra_eqos_data }, + { } + }; + MODULE_DEVICE_TABLE(of, dwc_eth_dwmac_match); +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c +@@ -74,6 +74,10 @@ struct rk_priv_data { + #define GRF_BIT(nr) (BIT(nr) | BIT(nr+16)) + #define GRF_CLR_BIT(nr) (BIT(nr+16)) + ++#define DELAY_ENABLE(soc, tx, rx) \ ++ (((tx) ? soc##_GMAC_TXCLK_DLY_ENABLE : soc##_GMAC_TXCLK_DLY_DISABLE) | \ ++ ((rx) ? soc##_GMAC_RXCLK_DLY_ENABLE : soc##_GMAC_RXCLK_DLY_DISABLE)) ++ + #define RK3228_GRF_MAC_CON0 0x0900 + #define RK3228_GRF_MAC_CON1 0x0904 + +@@ -115,8 +119,7 @@ static void rk3228_set_to_rgmii(struct r + regmap_write(bsp_priv->grf, RK3228_GRF_MAC_CON1, + RK3228_GMAC_PHY_INTF_SEL_RGMII | + RK3228_GMAC_RMII_MODE_CLR | +- RK3228_GMAC_RXCLK_DLY_ENABLE | +- RK3228_GMAC_TXCLK_DLY_ENABLE); ++ DELAY_ENABLE(RK3228, tx_delay, rx_delay)); + + regmap_write(bsp_priv->grf, RK3228_GRF_MAC_CON0, + RK3228_GMAC_CLK_RX_DL_CFG(rx_delay) | +@@ -232,8 +235,7 @@ static void rk3288_set_to_rgmii(struct r + RK3288_GMAC_PHY_INTF_SEL_RGMII | + RK3288_GMAC_RMII_MODE_CLR); + regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON3, +- RK3288_GMAC_RXCLK_DLY_ENABLE | +- RK3288_GMAC_TXCLK_DLY_ENABLE | ++ DELAY_ENABLE(RK3288, tx_delay, rx_delay) | + RK3288_GMAC_CLK_RX_DL_CFG(rx_delay) | + RK3288_GMAC_CLK_TX_DL_CFG(tx_delay)); + } +@@ -460,8 +462,7 @@ static void rk3366_set_to_rgmii(struct r + RK3366_GMAC_PHY_INTF_SEL_RGMII | + RK3366_GMAC_RMII_MODE_CLR); + regmap_write(bsp_priv->grf, RK3366_GRF_SOC_CON7, +- RK3366_GMAC_RXCLK_DLY_ENABLE | +- RK3366_GMAC_TXCLK_DLY_ENABLE | ++ DELAY_ENABLE(RK3366, tx_delay, rx_delay) | + RK3366_GMAC_CLK_RX_DL_CFG(rx_delay) | + RK3366_GMAC_CLK_TX_DL_CFG(tx_delay)); + } +@@ -572,8 +573,7 @@ static void rk3368_set_to_rgmii(struct r + RK3368_GMAC_PHY_INTF_SEL_RGMII | + RK3368_GMAC_RMII_MODE_CLR); + regmap_write(bsp_priv->grf, RK3368_GRF_SOC_CON16, +- RK3368_GMAC_RXCLK_DLY_ENABLE | +- RK3368_GMAC_TXCLK_DLY_ENABLE | ++ DELAY_ENABLE(RK3368, tx_delay, rx_delay) | + RK3368_GMAC_CLK_RX_DL_CFG(rx_delay) | + RK3368_GMAC_CLK_TX_DL_CFG(tx_delay)); + } +@@ -684,8 +684,7 @@ static void rk3399_set_to_rgmii(struct r + RK3399_GMAC_PHY_INTF_SEL_RGMII | + RK3399_GMAC_RMII_MODE_CLR); + regmap_write(bsp_priv->grf, RK3399_GRF_SOC_CON6, +- RK3399_GMAC_RXCLK_DLY_ENABLE | +- RK3399_GMAC_TXCLK_DLY_ENABLE | ++ DELAY_ENABLE(RK3399, tx_delay, rx_delay) | + RK3399_GMAC_CLK_RX_DL_CFG(rx_delay) | + RK3399_GMAC_CLK_TX_DL_CFG(tx_delay)); + } +@@ -985,14 +984,29 @@ static int rk_gmac_powerup(struct rk_pri + return ret; + + /*rmii or rgmii*/ +- if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RGMII) { ++ switch (bsp_priv->phy_iface) { ++ case PHY_INTERFACE_MODE_RGMII: + dev_info(dev, "init for RGMII\n"); + bsp_priv->ops->set_to_rgmii(bsp_priv, bsp_priv->tx_delay, + bsp_priv->rx_delay); +- } else if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RMII) { ++ break; ++ case PHY_INTERFACE_MODE_RGMII_ID: ++ dev_info(dev, "init for RGMII_ID\n"); ++ bsp_priv->ops->set_to_rgmii(bsp_priv, 0, 0); ++ break; ++ case PHY_INTERFACE_MODE_RGMII_RXID: ++ dev_info(dev, "init for RGMII_RXID\n"); ++ bsp_priv->ops->set_to_rgmii(bsp_priv, bsp_priv->tx_delay, 0); ++ break; ++ case PHY_INTERFACE_MODE_RGMII_TXID: ++ dev_info(dev, "init for RGMII_TXID\n"); ++ bsp_priv->ops->set_to_rgmii(bsp_priv, 0, bsp_priv->rx_delay); ++ break; ++ case PHY_INTERFACE_MODE_RMII: + dev_info(dev, "init for RMII\n"); + bsp_priv->ops->set_to_rmii(bsp_priv); +- } else { ++ break; ++ default: + dev_err(dev, "NO interface defined!\n"); + } + +@@ -1022,12 +1036,19 @@ static void rk_fix_speed(void *priv, uns + struct rk_priv_data *bsp_priv = priv; + struct device *dev = &bsp_priv->pdev->dev; + +- if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RGMII) ++ switch (bsp_priv->phy_iface) { ++ case PHY_INTERFACE_MODE_RGMII: ++ case PHY_INTERFACE_MODE_RGMII_ID: ++ case PHY_INTERFACE_MODE_RGMII_RXID: ++ case PHY_INTERFACE_MODE_RGMII_TXID: + bsp_priv->ops->set_rgmii_speed(bsp_priv, speed); +- else if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RMII) ++ break; ++ case PHY_INTERFACE_MODE_RMII: + bsp_priv->ops->set_rmii_speed(bsp_priv, speed); +- else ++ break; ++ default: + dev_err(dev, "unsupported interface %d", bsp_priv->phy_iface); ++ } + } + + static int rk_gmac_probe(struct platform_device *pdev) +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c +@@ -216,7 +216,8 @@ static void dwmac1000_set_filter(struct + + + static void dwmac1000_flow_ctrl(struct mac_device_info *hw, unsigned int duplex, +- unsigned int fc, unsigned int pause_time) ++ unsigned int fc, unsigned int pause_time, ++ u32 tx_cnt) + { + void __iomem *ioaddr = hw->pcsr; + /* Set flow such that DZPQ in Mac Register 6 is 0, +@@ -412,7 +413,8 @@ static void dwmac1000_get_adv_lp(void __ + dwmac_get_adv_lp(ioaddr, GMAC_PCS_BASE, adv); + } + +-static void dwmac1000_debug(void __iomem *ioaddr, struct stmmac_extra_stats *x) ++static void dwmac1000_debug(void __iomem *ioaddr, struct stmmac_extra_stats *x, ++ u32 rx_queues, u32 tx_queues) + { + u32 value = readl(ioaddr + GMAC_DEBUG); + +@@ -488,6 +490,7 @@ static void dwmac1000_debug(void __iomem + + static const struct stmmac_ops dwmac1000_ops = { + .core_init = dwmac1000_core_init, ++ .set_mac = stmmac_set_mac, + .rx_ipc = dwmac1000_rx_ipc_enable, + .dump_regs = dwmac1000_dump_regs, + .host_irq_status = dwmac1000_irq_status, +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c +@@ -247,7 +247,8 @@ static void dwmac1000_get_hw_feature(voi + dma_cap->enh_desc = (hw_cap & DMA_HW_FEAT_ENHDESSEL) >> 24; + } + +-static void dwmac1000_rx_watchdog(void __iomem *ioaddr, u32 riwt) ++static void dwmac1000_rx_watchdog(void __iomem *ioaddr, u32 riwt, ++ u32 number_chan) + { + writel(riwt, ioaddr + DMA_RX_WATCHDOG); + } +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c +@@ -131,7 +131,8 @@ static void dwmac100_set_filter(struct m + } + + static void dwmac100_flow_ctrl(struct mac_device_info *hw, unsigned int duplex, +- unsigned int fc, unsigned int pause_time) ++ unsigned int fc, unsigned int pause_time, ++ u32 tx_cnt) + { + void __iomem *ioaddr = hw->pcsr; + unsigned int flow = MAC_FLOW_CTRL_ENABLE; +@@ -149,6 +150,7 @@ static void dwmac100_pmt(struct mac_devi + + static const struct stmmac_ops dwmac100_ops = { + .core_init = dwmac100_core_init, ++ .set_mac = stmmac_set_mac, + .rx_ipc = dwmac100_rx_ipc_enable, + .dump_regs = dwmac100_dump_mac_regs, + .host_irq_status = dwmac100_irq_status, +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4.h ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4.h +@@ -22,9 +22,15 @@ + #define GMAC_HASH_TAB_32_63 0x00000014 + #define GMAC_RX_FLOW_CTRL 0x00000090 + #define GMAC_QX_TX_FLOW_CTRL(x) (0x70 + x * 4) ++#define GMAC_TXQ_PRTY_MAP0 0x98 ++#define GMAC_TXQ_PRTY_MAP1 0x9C + #define GMAC_RXQ_CTRL0 0x000000a0 ++#define GMAC_RXQ_CTRL1 0x000000a4 ++#define GMAC_RXQ_CTRL2 0x000000a8 ++#define GMAC_RXQ_CTRL3 0x000000ac + #define GMAC_INT_STATUS 0x000000b0 + #define GMAC_INT_EN 0x000000b4 ++#define GMAC_1US_TIC_COUNTER 0x000000dc + #define GMAC_PCS_BASE 0x000000e0 + #define GMAC_PHYIF_CONTROL_STATUS 0x000000f8 + #define GMAC_PMT 0x000000c0 +@@ -38,6 +44,22 @@ + #define GMAC_ADDR_HIGH(reg) (0x300 + reg * 8) + #define GMAC_ADDR_LOW(reg) (0x304 + reg * 8) + ++/* RX Queues Routing */ ++#define GMAC_RXQCTRL_AVCPQ_MASK GENMASK(2, 0) ++#define GMAC_RXQCTRL_AVCPQ_SHIFT 0 ++#define GMAC_RXQCTRL_PTPQ_MASK GENMASK(6, 4) ++#define GMAC_RXQCTRL_PTPQ_SHIFT 4 ++#define GMAC_RXQCTRL_DCBCPQ_MASK GENMASK(10, 8) ++#define GMAC_RXQCTRL_DCBCPQ_SHIFT 8 ++#define GMAC_RXQCTRL_UPQ_MASK GENMASK(14, 12) ++#define GMAC_RXQCTRL_UPQ_SHIFT 12 ++#define GMAC_RXQCTRL_MCBCQ_MASK GENMASK(18, 16) ++#define GMAC_RXQCTRL_MCBCQ_SHIFT 16 ++#define GMAC_RXQCTRL_MCBCQEN BIT(20) ++#define GMAC_RXQCTRL_MCBCQEN_SHIFT 20 ++#define GMAC_RXQCTRL_TACPQE BIT(21) ++#define GMAC_RXQCTRL_TACPQE_SHIFT 21 ++ + /* MAC Packet Filtering */ + #define GMAC_PACKET_FILTER_PR BIT(0) + #define GMAC_PACKET_FILTER_HMC BIT(2) +@@ -53,6 +75,14 @@ + /* MAC Flow Control RX */ + #define GMAC_RX_FLOW_CTRL_RFE BIT(0) + ++/* RX Queues Priorities */ ++#define GMAC_RXQCTRL_PSRQX_MASK(x) GENMASK(7 + ((x) * 8), 0 + ((x) * 8)) ++#define GMAC_RXQCTRL_PSRQX_SHIFT(x) ((x) * 8) ++ ++/* TX Queues Priorities */ ++#define GMAC_TXQCTRL_PSTQX_MASK(x) GENMASK(7 + ((x) * 8), 0 + ((x) * 8)) ++#define GMAC_TXQCTRL_PSTQX_SHIFT(x) ((x) * 8) ++ + /* MAC Flow Control TX */ + #define GMAC_TX_FLOW_CTRL_TFE BIT(1) + #define GMAC_TX_FLOW_CTRL_PT_SHIFT 16 +@@ -148,6 +178,8 @@ enum power_event { + /* MAC HW features1 bitmap */ + #define GMAC_HW_FEAT_AVSEL BIT(20) + #define GMAC_HW_TSOEN BIT(18) ++#define GMAC_HW_TXFIFOSIZE GENMASK(10, 6) ++#define GMAC_HW_RXFIFOSIZE GENMASK(4, 0) + + /* MAC HW features2 bitmap */ + #define GMAC_HW_FEAT_TXCHCNT GENMASK(21, 18) +@@ -161,8 +193,25 @@ enum power_event { + #define GMAC_HI_REG_AE BIT(31) + + /* MTL registers */ ++#define MTL_OPERATION_MODE 0x00000c00 ++#define MTL_OPERATION_SCHALG_MASK GENMASK(6, 5) ++#define MTL_OPERATION_SCHALG_WRR (0x0 << 5) ++#define MTL_OPERATION_SCHALG_WFQ (0x1 << 5) ++#define MTL_OPERATION_SCHALG_DWRR (0x2 << 5) ++#define MTL_OPERATION_SCHALG_SP (0x3 << 5) ++#define MTL_OPERATION_RAA BIT(2) ++#define MTL_OPERATION_RAA_SP (0x0 << 2) ++#define MTL_OPERATION_RAA_WSP (0x1 << 2) ++ + #define MTL_INT_STATUS 0x00000c20 +-#define MTL_INT_Q0 BIT(0) ++#define MTL_INT_QX(x) BIT(x) ++ ++#define MTL_RXQ_DMA_MAP0 0x00000c30 /* queue 0 to 3 */ ++#define MTL_RXQ_DMA_MAP1 0x00000c34 /* queue 4 to 7 */ ++#define MTL_RXQ_DMA_Q04MDMACH_MASK GENMASK(3, 0) ++#define MTL_RXQ_DMA_Q04MDMACH(x) ((x) << 0) ++#define MTL_RXQ_DMA_QXMDMACH_MASK(x) GENMASK(11 + (8 * ((x) - 1)), 8 * (x)) ++#define MTL_RXQ_DMA_QXMDMACH(chan, q) ((chan) << (8 * (q))) + + #define MTL_CHAN_BASE_ADDR 0x00000d00 + #define MTL_CHAN_BASE_OFFSET 0x40 +@@ -180,6 +229,7 @@ enum power_event { + #define MTL_OP_MODE_TSF BIT(1) + + #define MTL_OP_MODE_TQS_MASK GENMASK(24, 16) ++#define MTL_OP_MODE_TQS_SHIFT 16 + + #define MTL_OP_MODE_TTC_MASK 0x70 + #define MTL_OP_MODE_TTC_SHIFT 4 +@@ -193,6 +243,17 @@ enum power_event { + #define MTL_OP_MODE_TTC_384 (6 << MTL_OP_MODE_TTC_SHIFT) + #define MTL_OP_MODE_TTC_512 (7 << MTL_OP_MODE_TTC_SHIFT) + ++#define MTL_OP_MODE_RQS_MASK GENMASK(29, 20) ++#define MTL_OP_MODE_RQS_SHIFT 20 ++ ++#define MTL_OP_MODE_RFD_MASK GENMASK(19, 14) ++#define MTL_OP_MODE_RFD_SHIFT 14 ++ ++#define MTL_OP_MODE_RFA_MASK GENMASK(13, 8) ++#define MTL_OP_MODE_RFA_SHIFT 8 ++ ++#define MTL_OP_MODE_EHFC BIT(7) ++ + #define MTL_OP_MODE_RTC_MASK 0x18 + #define MTL_OP_MODE_RTC_SHIFT 3 + +@@ -201,6 +262,46 @@ enum power_event { + #define MTL_OP_MODE_RTC_96 (2 << MTL_OP_MODE_RTC_SHIFT) + #define MTL_OP_MODE_RTC_128 (3 << MTL_OP_MODE_RTC_SHIFT) + ++/* MTL ETS Control register */ ++#define MTL_ETS_CTRL_BASE_ADDR 0x00000d10 ++#define MTL_ETS_CTRL_BASE_OFFSET 0x40 ++#define MTL_ETSX_CTRL_BASE_ADDR(x) (MTL_ETS_CTRL_BASE_ADDR + \ ++ ((x) * MTL_ETS_CTRL_BASE_OFFSET)) ++ ++#define MTL_ETS_CTRL_CC BIT(3) ++#define MTL_ETS_CTRL_AVALG BIT(2) ++ ++/* MTL Queue Quantum Weight */ ++#define MTL_TXQ_WEIGHT_BASE_ADDR 0x00000d18 ++#define MTL_TXQ_WEIGHT_BASE_OFFSET 0x40 ++#define MTL_TXQX_WEIGHT_BASE_ADDR(x) (MTL_TXQ_WEIGHT_BASE_ADDR + \ ++ ((x) * MTL_TXQ_WEIGHT_BASE_OFFSET)) ++#define MTL_TXQ_WEIGHT_ISCQW_MASK GENMASK(20, 0) ++ ++/* MTL sendSlopeCredit register */ ++#define MTL_SEND_SLP_CRED_BASE_ADDR 0x00000d1c ++#define MTL_SEND_SLP_CRED_OFFSET 0x40 ++#define MTL_SEND_SLP_CREDX_BASE_ADDR(x) (MTL_SEND_SLP_CRED_BASE_ADDR + \ ++ ((x) * MTL_SEND_SLP_CRED_OFFSET)) ++ ++#define MTL_SEND_SLP_CRED_SSC_MASK GENMASK(13, 0) ++ ++/* MTL hiCredit register */ ++#define MTL_HIGH_CRED_BASE_ADDR 0x00000d20 ++#define MTL_HIGH_CRED_OFFSET 0x40 ++#define MTL_HIGH_CREDX_BASE_ADDR(x) (MTL_HIGH_CRED_BASE_ADDR + \ ++ ((x) * MTL_HIGH_CRED_OFFSET)) ++ ++#define MTL_HIGH_CRED_HC_MASK GENMASK(28, 0) ++ ++/* MTL loCredit register */ ++#define MTL_LOW_CRED_BASE_ADDR 0x00000d24 ++#define MTL_LOW_CRED_OFFSET 0x40 ++#define MTL_LOW_CREDX_BASE_ADDR(x) (MTL_LOW_CRED_BASE_ADDR + \ ++ ((x) * MTL_LOW_CRED_OFFSET)) ++ ++#define MTL_HIGH_CRED_LC_MASK GENMASK(28, 0) ++ + /* MTL debug */ + #define MTL_DEBUG_TXSTSFSTS BIT(5) + #define MTL_DEBUG_TXFSTS BIT(4) +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c +@@ -59,17 +59,211 @@ static void dwmac4_core_init(struct mac_ + writel(value, ioaddr + GMAC_INT_EN); + } + +-static void dwmac4_rx_queue_enable(struct mac_device_info *hw, u32 queue) ++static void dwmac4_rx_queue_enable(struct mac_device_info *hw, ++ u8 mode, u32 queue) + { + void __iomem *ioaddr = hw->pcsr; + u32 value = readl(ioaddr + GMAC_RXQ_CTRL0); + + value &= GMAC_RX_QUEUE_CLEAR(queue); +- value |= GMAC_RX_AV_QUEUE_ENABLE(queue); ++ if (mode == MTL_QUEUE_AVB) ++ value |= GMAC_RX_AV_QUEUE_ENABLE(queue); ++ else if (mode == MTL_QUEUE_DCB) ++ value |= GMAC_RX_DCB_QUEUE_ENABLE(queue); + + writel(value, ioaddr + GMAC_RXQ_CTRL0); + } + ++static void dwmac4_rx_queue_priority(struct mac_device_info *hw, ++ u32 prio, u32 queue) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 base_register; ++ u32 value; ++ ++ base_register = (queue < 4) ? GMAC_RXQ_CTRL2 : GMAC_RXQ_CTRL3; ++ ++ value = readl(ioaddr + base_register); ++ ++ value &= ~GMAC_RXQCTRL_PSRQX_MASK(queue); ++ value |= (prio << GMAC_RXQCTRL_PSRQX_SHIFT(queue)) & ++ GMAC_RXQCTRL_PSRQX_MASK(queue); ++ writel(value, ioaddr + base_register); ++} ++ ++static void dwmac4_tx_queue_priority(struct mac_device_info *hw, ++ u32 prio, u32 queue) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 base_register; ++ u32 value; ++ ++ base_register = (queue < 4) ? GMAC_TXQ_PRTY_MAP0 : GMAC_TXQ_PRTY_MAP1; ++ ++ value = readl(ioaddr + base_register); ++ ++ value &= ~GMAC_TXQCTRL_PSTQX_MASK(queue); ++ value |= (prio << GMAC_TXQCTRL_PSTQX_SHIFT(queue)) & ++ GMAC_TXQCTRL_PSTQX_MASK(queue); ++ ++ writel(value, ioaddr + base_register); ++} ++ ++static void dwmac4_tx_queue_routing(struct mac_device_info *hw, ++ u8 packet, u32 queue) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 value; ++ ++ const struct stmmac_rx_routing route_possibilities[] = { ++ { GMAC_RXQCTRL_AVCPQ_MASK, GMAC_RXQCTRL_AVCPQ_SHIFT }, ++ { GMAC_RXQCTRL_PTPQ_MASK, GMAC_RXQCTRL_PTPQ_SHIFT }, ++ { GMAC_RXQCTRL_DCBCPQ_MASK, GMAC_RXQCTRL_DCBCPQ_SHIFT }, ++ { GMAC_RXQCTRL_UPQ_MASK, GMAC_RXQCTRL_UPQ_SHIFT }, ++ { GMAC_RXQCTRL_MCBCQ_MASK, GMAC_RXQCTRL_MCBCQ_SHIFT }, ++ }; ++ ++ value = readl(ioaddr + GMAC_RXQ_CTRL1); ++ ++ /* routing configuration */ ++ value &= ~route_possibilities[packet - 1].reg_mask; ++ value |= (queue << route_possibilities[packet-1].reg_shift) & ++ route_possibilities[packet - 1].reg_mask; ++ ++ /* some packets require extra ops */ ++ if (packet == PACKET_AVCPQ) { ++ value &= ~GMAC_RXQCTRL_TACPQE; ++ value |= 0x1 << GMAC_RXQCTRL_TACPQE_SHIFT; ++ } else if (packet == PACKET_MCBCQ) { ++ value &= ~GMAC_RXQCTRL_MCBCQEN; ++ value |= 0x1 << GMAC_RXQCTRL_MCBCQEN_SHIFT; ++ } ++ ++ writel(value, ioaddr + GMAC_RXQ_CTRL1); ++} ++ ++static void dwmac4_prog_mtl_rx_algorithms(struct mac_device_info *hw, ++ u32 rx_alg) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 value = readl(ioaddr + MTL_OPERATION_MODE); ++ ++ value &= ~MTL_OPERATION_RAA; ++ switch (rx_alg) { ++ case MTL_RX_ALGORITHM_SP: ++ value |= MTL_OPERATION_RAA_SP; ++ break; ++ case MTL_RX_ALGORITHM_WSP: ++ value |= MTL_OPERATION_RAA_WSP; ++ break; ++ default: ++ break; ++ } ++ ++ writel(value, ioaddr + MTL_OPERATION_MODE); ++} ++ ++static void dwmac4_prog_mtl_tx_algorithms(struct mac_device_info *hw, ++ u32 tx_alg) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 value = readl(ioaddr + MTL_OPERATION_MODE); ++ ++ value &= ~MTL_OPERATION_SCHALG_MASK; ++ switch (tx_alg) { ++ case MTL_TX_ALGORITHM_WRR: ++ value |= MTL_OPERATION_SCHALG_WRR; ++ break; ++ case MTL_TX_ALGORITHM_WFQ: ++ value |= MTL_OPERATION_SCHALG_WFQ; ++ break; ++ case MTL_TX_ALGORITHM_DWRR: ++ value |= MTL_OPERATION_SCHALG_DWRR; ++ break; ++ case MTL_TX_ALGORITHM_SP: ++ value |= MTL_OPERATION_SCHALG_SP; ++ break; ++ default: ++ break; ++ } ++} ++ ++static void dwmac4_set_mtl_tx_queue_weight(struct mac_device_info *hw, ++ u32 weight, u32 queue) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 value = readl(ioaddr + MTL_TXQX_WEIGHT_BASE_ADDR(queue)); ++ ++ value &= ~MTL_TXQ_WEIGHT_ISCQW_MASK; ++ value |= weight & MTL_TXQ_WEIGHT_ISCQW_MASK; ++ writel(value, ioaddr + MTL_TXQX_WEIGHT_BASE_ADDR(queue)); ++} ++ ++static void dwmac4_map_mtl_dma(struct mac_device_info *hw, u32 queue, u32 chan) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 value; ++ ++ if (queue < 4) ++ value = readl(ioaddr + MTL_RXQ_DMA_MAP0); ++ else ++ value = readl(ioaddr + MTL_RXQ_DMA_MAP1); ++ ++ if (queue == 0 || queue == 4) { ++ value &= ~MTL_RXQ_DMA_Q04MDMACH_MASK; ++ value |= MTL_RXQ_DMA_Q04MDMACH(chan); ++ } else { ++ value &= ~MTL_RXQ_DMA_QXMDMACH_MASK(queue); ++ value |= MTL_RXQ_DMA_QXMDMACH(chan, queue); ++ } ++ ++ if (queue < 4) ++ writel(value, ioaddr + MTL_RXQ_DMA_MAP0); ++ else ++ writel(value, ioaddr + MTL_RXQ_DMA_MAP1); ++} ++ ++static void dwmac4_config_cbs(struct mac_device_info *hw, ++ u32 send_slope, u32 idle_slope, ++ u32 high_credit, u32 low_credit, u32 queue) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 value; ++ ++ pr_debug("Queue %d configured as AVB. Parameters:\n", queue); ++ pr_debug("\tsend_slope: 0x%08x\n", send_slope); ++ pr_debug("\tidle_slope: 0x%08x\n", idle_slope); ++ pr_debug("\thigh_credit: 0x%08x\n", high_credit); ++ pr_debug("\tlow_credit: 0x%08x\n", low_credit); ++ ++ /* enable AV algorithm */ ++ value = readl(ioaddr + MTL_ETSX_CTRL_BASE_ADDR(queue)); ++ value |= MTL_ETS_CTRL_AVALG; ++ value |= MTL_ETS_CTRL_CC; ++ writel(value, ioaddr + MTL_ETSX_CTRL_BASE_ADDR(queue)); ++ ++ /* configure send slope */ ++ value = readl(ioaddr + MTL_SEND_SLP_CREDX_BASE_ADDR(queue)); ++ value &= ~MTL_SEND_SLP_CRED_SSC_MASK; ++ value |= send_slope & MTL_SEND_SLP_CRED_SSC_MASK; ++ writel(value, ioaddr + MTL_SEND_SLP_CREDX_BASE_ADDR(queue)); ++ ++ /* configure idle slope (same register as tx weight) */ ++ dwmac4_set_mtl_tx_queue_weight(hw, idle_slope, queue); ++ ++ /* configure high credit */ ++ value = readl(ioaddr + MTL_HIGH_CREDX_BASE_ADDR(queue)); ++ value &= ~MTL_HIGH_CRED_HC_MASK; ++ value |= high_credit & MTL_HIGH_CRED_HC_MASK; ++ writel(value, ioaddr + MTL_HIGH_CREDX_BASE_ADDR(queue)); ++ ++ /* configure high credit */ ++ value = readl(ioaddr + MTL_LOW_CREDX_BASE_ADDR(queue)); ++ value &= ~MTL_HIGH_CRED_LC_MASK; ++ value |= low_credit & MTL_HIGH_CRED_LC_MASK; ++ writel(value, ioaddr + MTL_LOW_CREDX_BASE_ADDR(queue)); ++} ++ + static void dwmac4_dump_regs(struct mac_device_info *hw, u32 *reg_space) + { + void __iomem *ioaddr = hw->pcsr; +@@ -251,11 +445,12 @@ static void dwmac4_set_filter(struct mac + } + + static void dwmac4_flow_ctrl(struct mac_device_info *hw, unsigned int duplex, +- unsigned int fc, unsigned int pause_time) ++ unsigned int fc, unsigned int pause_time, ++ u32 tx_cnt) + { + void __iomem *ioaddr = hw->pcsr; +- u32 channel = STMMAC_CHAN0; /* FIXME */ + unsigned int flow = 0; ++ u32 queue = 0; + + pr_debug("GMAC Flow-Control:\n"); + if (fc & FLOW_RX) { +@@ -265,13 +460,18 @@ static void dwmac4_flow_ctrl(struct mac_ + } + if (fc & FLOW_TX) { + pr_debug("\tTransmit Flow-Control ON\n"); +- flow |= GMAC_TX_FLOW_CTRL_TFE; +- writel(flow, ioaddr + GMAC_QX_TX_FLOW_CTRL(channel)); + +- if (duplex) { ++ if (duplex) + pr_debug("\tduplex mode: PAUSE %d\n", pause_time); +- flow |= (pause_time << GMAC_TX_FLOW_CTRL_PT_SHIFT); +- writel(flow, ioaddr + GMAC_QX_TX_FLOW_CTRL(channel)); ++ ++ for (queue = 0; queue < tx_cnt; queue++) { ++ flow |= GMAC_TX_FLOW_CTRL_TFE; ++ ++ if (duplex) ++ flow |= ++ (pause_time << GMAC_TX_FLOW_CTRL_PT_SHIFT); ++ ++ writel(flow, ioaddr + GMAC_QX_TX_FLOW_CTRL(queue)); + } + } + } +@@ -325,11 +525,34 @@ static void dwmac4_phystatus(void __iome + } + } + ++static int dwmac4_irq_mtl_status(struct mac_device_info *hw, u32 chan) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 mtl_int_qx_status; ++ int ret = 0; ++ ++ mtl_int_qx_status = readl(ioaddr + MTL_INT_STATUS); ++ ++ /* Check MTL Interrupt */ ++ if (mtl_int_qx_status & MTL_INT_QX(chan)) { ++ /* read Queue x Interrupt status */ ++ u32 status = readl(ioaddr + MTL_CHAN_INT_CTRL(chan)); ++ ++ if (status & MTL_RX_OVERFLOW_INT) { ++ /* clear Interrupt */ ++ writel(status | MTL_RX_OVERFLOW_INT, ++ ioaddr + MTL_CHAN_INT_CTRL(chan)); ++ ret = CORE_IRQ_MTL_RX_OVERFLOW; ++ } ++ } ++ ++ return ret; ++} ++ + static int dwmac4_irq_status(struct mac_device_info *hw, + struct stmmac_extra_stats *x) + { + void __iomem *ioaddr = hw->pcsr; +- u32 mtl_int_qx_status; + u32 intr_status; + int ret = 0; + +@@ -348,20 +571,6 @@ static int dwmac4_irq_status(struct mac_ + x->irq_receive_pmt_irq_n++; + } + +- mtl_int_qx_status = readl(ioaddr + MTL_INT_STATUS); +- /* Check MTL Interrupt: Currently only one queue is used: Q0. */ +- if (mtl_int_qx_status & MTL_INT_Q0) { +- /* read Queue 0 Interrupt status */ +- u32 status = readl(ioaddr + MTL_CHAN_INT_CTRL(STMMAC_CHAN0)); +- +- if (status & MTL_RX_OVERFLOW_INT) { +- /* clear Interrupt */ +- writel(status | MTL_RX_OVERFLOW_INT, +- ioaddr + MTL_CHAN_INT_CTRL(STMMAC_CHAN0)); +- ret = CORE_IRQ_MTL_RX_OVERFLOW; +- } +- } +- + dwmac_pcs_isr(ioaddr, GMAC_PCS_BASE, intr_status, x); + if (intr_status & PCS_RGSMIIIS_IRQ) + dwmac4_phystatus(ioaddr, x); +@@ -369,64 +578,69 @@ static int dwmac4_irq_status(struct mac_ + return ret; + } + +-static void dwmac4_debug(void __iomem *ioaddr, struct stmmac_extra_stats *x) ++static void dwmac4_debug(void __iomem *ioaddr, struct stmmac_extra_stats *x, ++ u32 rx_queues, u32 tx_queues) + { + u32 value; ++ u32 queue; + +- /* Currently only channel 0 is supported */ +- value = readl(ioaddr + MTL_CHAN_TX_DEBUG(STMMAC_CHAN0)); ++ for (queue = 0; queue < tx_queues; queue++) { ++ value = readl(ioaddr + MTL_CHAN_TX_DEBUG(queue)); + +- if (value & MTL_DEBUG_TXSTSFSTS) +- x->mtl_tx_status_fifo_full++; +- if (value & MTL_DEBUG_TXFSTS) +- x->mtl_tx_fifo_not_empty++; +- if (value & MTL_DEBUG_TWCSTS) +- x->mmtl_fifo_ctrl++; +- if (value & MTL_DEBUG_TRCSTS_MASK) { +- u32 trcsts = (value & MTL_DEBUG_TRCSTS_MASK) +- >> MTL_DEBUG_TRCSTS_SHIFT; +- if (trcsts == MTL_DEBUG_TRCSTS_WRITE) +- x->mtl_tx_fifo_read_ctrl_write++; +- else if (trcsts == MTL_DEBUG_TRCSTS_TXW) +- x->mtl_tx_fifo_read_ctrl_wait++; +- else if (trcsts == MTL_DEBUG_TRCSTS_READ) +- x->mtl_tx_fifo_read_ctrl_read++; +- else +- x->mtl_tx_fifo_read_ctrl_idle++; ++ if (value & MTL_DEBUG_TXSTSFSTS) ++ x->mtl_tx_status_fifo_full++; ++ if (value & MTL_DEBUG_TXFSTS) ++ x->mtl_tx_fifo_not_empty++; ++ if (value & MTL_DEBUG_TWCSTS) ++ x->mmtl_fifo_ctrl++; ++ if (value & MTL_DEBUG_TRCSTS_MASK) { ++ u32 trcsts = (value & MTL_DEBUG_TRCSTS_MASK) ++ >> MTL_DEBUG_TRCSTS_SHIFT; ++ if (trcsts == MTL_DEBUG_TRCSTS_WRITE) ++ x->mtl_tx_fifo_read_ctrl_write++; ++ else if (trcsts == MTL_DEBUG_TRCSTS_TXW) ++ x->mtl_tx_fifo_read_ctrl_wait++; ++ else if (trcsts == MTL_DEBUG_TRCSTS_READ) ++ x->mtl_tx_fifo_read_ctrl_read++; ++ else ++ x->mtl_tx_fifo_read_ctrl_idle++; ++ } ++ if (value & MTL_DEBUG_TXPAUSED) ++ x->mac_tx_in_pause++; + } +- if (value & MTL_DEBUG_TXPAUSED) +- x->mac_tx_in_pause++; + +- value = readl(ioaddr + MTL_CHAN_RX_DEBUG(STMMAC_CHAN0)); ++ for (queue = 0; queue < rx_queues; queue++) { ++ value = readl(ioaddr + MTL_CHAN_RX_DEBUG(queue)); + +- if (value & MTL_DEBUG_RXFSTS_MASK) { +- u32 rxfsts = (value & MTL_DEBUG_RXFSTS_MASK) +- >> MTL_DEBUG_RRCSTS_SHIFT; +- +- if (rxfsts == MTL_DEBUG_RXFSTS_FULL) +- x->mtl_rx_fifo_fill_level_full++; +- else if (rxfsts == MTL_DEBUG_RXFSTS_AT) +- x->mtl_rx_fifo_fill_above_thresh++; +- else if (rxfsts == MTL_DEBUG_RXFSTS_BT) +- x->mtl_rx_fifo_fill_below_thresh++; +- else +- x->mtl_rx_fifo_fill_level_empty++; +- } +- if (value & MTL_DEBUG_RRCSTS_MASK) { +- u32 rrcsts = (value & MTL_DEBUG_RRCSTS_MASK) >> +- MTL_DEBUG_RRCSTS_SHIFT; +- +- if (rrcsts == MTL_DEBUG_RRCSTS_FLUSH) +- x->mtl_rx_fifo_read_ctrl_flush++; +- else if (rrcsts == MTL_DEBUG_RRCSTS_RSTAT) +- x->mtl_rx_fifo_read_ctrl_read_data++; +- else if (rrcsts == MTL_DEBUG_RRCSTS_RDATA) +- x->mtl_rx_fifo_read_ctrl_status++; +- else +- x->mtl_rx_fifo_read_ctrl_idle++; ++ if (value & MTL_DEBUG_RXFSTS_MASK) { ++ u32 rxfsts = (value & MTL_DEBUG_RXFSTS_MASK) ++ >> MTL_DEBUG_RRCSTS_SHIFT; ++ ++ if (rxfsts == MTL_DEBUG_RXFSTS_FULL) ++ x->mtl_rx_fifo_fill_level_full++; ++ else if (rxfsts == MTL_DEBUG_RXFSTS_AT) ++ x->mtl_rx_fifo_fill_above_thresh++; ++ else if (rxfsts == MTL_DEBUG_RXFSTS_BT) ++ x->mtl_rx_fifo_fill_below_thresh++; ++ else ++ x->mtl_rx_fifo_fill_level_empty++; ++ } ++ if (value & MTL_DEBUG_RRCSTS_MASK) { ++ u32 rrcsts = (value & MTL_DEBUG_RRCSTS_MASK) >> ++ MTL_DEBUG_RRCSTS_SHIFT; ++ ++ if (rrcsts == MTL_DEBUG_RRCSTS_FLUSH) ++ x->mtl_rx_fifo_read_ctrl_flush++; ++ else if (rrcsts == MTL_DEBUG_RRCSTS_RSTAT) ++ x->mtl_rx_fifo_read_ctrl_read_data++; ++ else if (rrcsts == MTL_DEBUG_RRCSTS_RDATA) ++ x->mtl_rx_fifo_read_ctrl_status++; ++ else ++ x->mtl_rx_fifo_read_ctrl_idle++; ++ } ++ if (value & MTL_DEBUG_RWCSTS) ++ x->mtl_rx_fifo_ctrl_active++; + } +- if (value & MTL_DEBUG_RWCSTS) +- x->mtl_rx_fifo_ctrl_active++; + + /* GMAC debug */ + value = readl(ioaddr + GMAC_DEBUG); +@@ -455,10 +669,51 @@ static void dwmac4_debug(void __iomem *i + + static const struct stmmac_ops dwmac4_ops = { + .core_init = dwmac4_core_init, ++ .set_mac = stmmac_set_mac, + .rx_ipc = dwmac4_rx_ipc_enable, + .rx_queue_enable = dwmac4_rx_queue_enable, ++ .rx_queue_prio = dwmac4_rx_queue_priority, ++ .tx_queue_prio = dwmac4_tx_queue_priority, ++ .rx_queue_routing = dwmac4_tx_queue_routing, ++ .prog_mtl_rx_algorithms = dwmac4_prog_mtl_rx_algorithms, ++ .prog_mtl_tx_algorithms = dwmac4_prog_mtl_tx_algorithms, ++ .set_mtl_tx_queue_weight = dwmac4_set_mtl_tx_queue_weight, ++ .map_mtl_to_dma = dwmac4_map_mtl_dma, ++ .config_cbs = dwmac4_config_cbs, + .dump_regs = dwmac4_dump_regs, + .host_irq_status = dwmac4_irq_status, ++ .host_mtl_irq_status = dwmac4_irq_mtl_status, ++ .flow_ctrl = dwmac4_flow_ctrl, ++ .pmt = dwmac4_pmt, ++ .set_umac_addr = dwmac4_set_umac_addr, ++ .get_umac_addr = dwmac4_get_umac_addr, ++ .set_eee_mode = dwmac4_set_eee_mode, ++ .reset_eee_mode = dwmac4_reset_eee_mode, ++ .set_eee_timer = dwmac4_set_eee_timer, ++ .set_eee_pls = dwmac4_set_eee_pls, ++ .pcs_ctrl_ane = dwmac4_ctrl_ane, ++ .pcs_rane = dwmac4_rane, ++ .pcs_get_adv_lp = dwmac4_get_adv_lp, ++ .debug = dwmac4_debug, ++ .set_filter = dwmac4_set_filter, ++}; ++ ++static const struct stmmac_ops dwmac410_ops = { ++ .core_init = dwmac4_core_init, ++ .set_mac = stmmac_dwmac4_set_mac, ++ .rx_ipc = dwmac4_rx_ipc_enable, ++ .rx_queue_enable = dwmac4_rx_queue_enable, ++ .rx_queue_prio = dwmac4_rx_queue_priority, ++ .tx_queue_prio = dwmac4_tx_queue_priority, ++ .rx_queue_routing = dwmac4_tx_queue_routing, ++ .prog_mtl_rx_algorithms = dwmac4_prog_mtl_rx_algorithms, ++ .prog_mtl_tx_algorithms = dwmac4_prog_mtl_tx_algorithms, ++ .set_mtl_tx_queue_weight = dwmac4_set_mtl_tx_queue_weight, ++ .map_mtl_to_dma = dwmac4_map_mtl_dma, ++ .config_cbs = dwmac4_config_cbs, ++ .dump_regs = dwmac4_dump_regs, ++ .host_irq_status = dwmac4_irq_status, ++ .host_mtl_irq_status = dwmac4_irq_mtl_status, + .flow_ctrl = dwmac4_flow_ctrl, + .pmt = dwmac4_pmt, + .set_umac_addr = dwmac4_set_umac_addr, +@@ -492,8 +747,6 @@ struct mac_device_info *dwmac4_setup(voi + if (mac->multicast_filter_bins) + mac->mcast_bits_log2 = ilog2(mac->multicast_filter_bins); + +- mac->mac = &dwmac4_ops; +- + mac->link.port = GMAC_CONFIG_PS; + mac->link.duplex = GMAC_CONFIG_DM; + mac->link.speed = GMAC_CONFIG_FES; +@@ -514,5 +767,10 @@ struct mac_device_info *dwmac4_setup(voi + else + mac->dma = &dwmac4_dma_ops; + ++ if (*synopsys_id >= DWMAC_CORE_4_00) ++ mac->mac = &dwmac410_ops; ++ else ++ mac->mac = &dwmac4_ops; ++ + return mac; + } +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c +@@ -214,13 +214,13 @@ static int dwmac4_wrback_get_tx_timestam + { + /* Context type from W/B descriptor must be zero */ + if (le32_to_cpu(p->des3) & TDES3_CONTEXT_TYPE) +- return -EINVAL; ++ return 0; + + /* Tx Timestamp Status is 1 so des0 and des1'll have valid values */ + if (le32_to_cpu(p->des3) & TDES3_TIMESTAMP_STATUS) +- return 0; ++ return 1; + +- return 1; ++ return 0; + } + + static inline u64 dwmac4_get_timestamp(void *desc, u32 ats) +@@ -282,7 +282,10 @@ static int dwmac4_wrback_get_rx_timestam + } + } + exit: +- return ret; ++ if (likely(ret == 0)) ++ return 1; ++ ++ return 0; + } + + static void dwmac4_rd_init_rx_desc(struct dma_desc *p, int disable_rx_ic, +@@ -304,12 +307,13 @@ static void dwmac4_rd_init_tx_desc(struc + + static void dwmac4_rd_prepare_tx_desc(struct dma_desc *p, int is_fs, int len, + bool csum_flag, int mode, bool tx_own, +- bool ls) ++ bool ls, unsigned int tot_pkt_len) + { + unsigned int tdes3 = le32_to_cpu(p->des3); + + p->des2 |= cpu_to_le32(len & TDES2_BUFFER1_SIZE_MASK); + ++ tdes3 |= tot_pkt_len & TDES3_PACKET_SIZE_MASK; + if (is_fs) + tdes3 |= TDES3_FIRST_DESCRIPTOR; + else +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c +@@ -71,36 +71,48 @@ static void dwmac4_dma_axi(void __iomem + writel(value, ioaddr + DMA_SYS_BUS_MODE); + } + +-static void dwmac4_dma_init_channel(void __iomem *ioaddr, +- struct stmmac_dma_cfg *dma_cfg, +- u32 dma_tx_phy, u32 dma_rx_phy, +- u32 channel) ++void dwmac4_dma_init_rx_chan(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, ++ u32 dma_rx_phy, u32 chan) + { + u32 value; +- int txpbl = dma_cfg->txpbl ?: dma_cfg->pbl; +- int rxpbl = dma_cfg->rxpbl ?: dma_cfg->pbl; ++ u32 rxpbl = dma_cfg->rxpbl ?: dma_cfg->pbl; + +- /* set PBL for each channels. Currently we affect same configuration +- * on each channel +- */ +- value = readl(ioaddr + DMA_CHAN_CONTROL(channel)); +- if (dma_cfg->pblx8) +- value = value | DMA_BUS_MODE_PBL; +- writel(value, ioaddr + DMA_CHAN_CONTROL(channel)); ++ value = readl(ioaddr + DMA_CHAN_RX_CONTROL(chan)); ++ value = value | (rxpbl << DMA_BUS_MODE_RPBL_SHIFT); ++ writel(value, ioaddr + DMA_CHAN_RX_CONTROL(chan)); ++ ++ writel(dma_rx_phy, ioaddr + DMA_CHAN_RX_BASE_ADDR(chan)); ++} + +- value = readl(ioaddr + DMA_CHAN_TX_CONTROL(channel)); ++void dwmac4_dma_init_tx_chan(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, ++ u32 dma_tx_phy, u32 chan) ++{ ++ u32 value; ++ u32 txpbl = dma_cfg->txpbl ?: dma_cfg->pbl; ++ ++ value = readl(ioaddr + DMA_CHAN_TX_CONTROL(chan)); + value = value | (txpbl << DMA_BUS_MODE_PBL_SHIFT); +- writel(value, ioaddr + DMA_CHAN_TX_CONTROL(channel)); ++ writel(value, ioaddr + DMA_CHAN_TX_CONTROL(chan)); + +- value = readl(ioaddr + DMA_CHAN_RX_CONTROL(channel)); +- value = value | (rxpbl << DMA_BUS_MODE_RPBL_SHIFT); +- writel(value, ioaddr + DMA_CHAN_RX_CONTROL(channel)); ++ writel(dma_tx_phy, ioaddr + DMA_CHAN_TX_BASE_ADDR(chan)); ++} + +- /* Mask interrupts by writing to CSR7 */ +- writel(DMA_CHAN_INTR_DEFAULT_MASK, ioaddr + DMA_CHAN_INTR_ENA(channel)); ++void dwmac4_dma_init_channel(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, u32 chan) ++{ ++ u32 value; ++ ++ /* common channel control register config */ ++ value = readl(ioaddr + DMA_CHAN_CONTROL(chan)); ++ if (dma_cfg->pblx8) ++ value = value | DMA_BUS_MODE_PBL; ++ writel(value, ioaddr + DMA_CHAN_CONTROL(chan)); + +- writel(dma_tx_phy, ioaddr + DMA_CHAN_TX_BASE_ADDR(channel)); +- writel(dma_rx_phy, ioaddr + DMA_CHAN_RX_BASE_ADDR(channel)); ++ /* Mask interrupts by writing to CSR7 */ ++ writel(DMA_CHAN_INTR_DEFAULT_MASK, ++ ioaddr + DMA_CHAN_INTR_ENA(chan)); + } + + static void dwmac4_dma_init(void __iomem *ioaddr, +@@ -108,7 +120,6 @@ static void dwmac4_dma_init(void __iomem + u32 dma_tx, u32 dma_rx, int atds) + { + u32 value = readl(ioaddr + DMA_SYS_BUS_MODE); +- int i; + + /* Set the Fixed burst mode */ + if (dma_cfg->fixed_burst) +@@ -122,9 +133,6 @@ static void dwmac4_dma_init(void __iomem + value |= DMA_SYS_BUS_AAL; + + writel(value, ioaddr + DMA_SYS_BUS_MODE); +- +- for (i = 0; i < DMA_CHANNEL_NB_MAX; i++) +- dwmac4_dma_init_channel(ioaddr, dma_cfg, dma_tx, dma_rx, i); + } + + static void _dwmac4_dump_dma_regs(void __iomem *ioaddr, u32 channel, +@@ -174,46 +182,121 @@ static void dwmac4_dump_dma_regs(void __ + _dwmac4_dump_dma_regs(ioaddr, i, reg_space); + } + +-static void dwmac4_rx_watchdog(void __iomem *ioaddr, u32 riwt) ++static void dwmac4_rx_watchdog(void __iomem *ioaddr, u32 riwt, u32 number_chan) + { +- int i; ++ u32 chan; + +- for (i = 0; i < DMA_CHANNEL_NB_MAX; i++) +- writel(riwt, ioaddr + DMA_CHAN_RX_WATCHDOG(i)); ++ for (chan = 0; chan < number_chan; chan++) ++ writel(riwt, ioaddr + DMA_CHAN_RX_WATCHDOG(chan)); + } + +-static void dwmac4_dma_chan_op_mode(void __iomem *ioaddr, int txmode, +- int rxmode, u32 channel) ++static void dwmac4_dma_rx_chan_op_mode(void __iomem *ioaddr, int mode, ++ u32 channel, int fifosz) + { +- u32 mtl_tx_op, mtl_rx_op, mtl_rx_int; ++ unsigned int rqs = fifosz / 256 - 1; ++ u32 mtl_rx_op, mtl_rx_int; + +- /* Following code only done for channel 0, other channels not yet +- * supported. +- */ +- mtl_tx_op = readl(ioaddr + MTL_CHAN_TX_OP_MODE(channel)); ++ mtl_rx_op = readl(ioaddr + MTL_CHAN_RX_OP_MODE(channel)); ++ ++ if (mode == SF_DMA_MODE) { ++ pr_debug("GMAC: enable RX store and forward mode\n"); ++ mtl_rx_op |= MTL_OP_MODE_RSF; ++ } else { ++ pr_debug("GMAC: disable RX SF mode (threshold %d)\n", mode); ++ mtl_rx_op &= ~MTL_OP_MODE_RSF; ++ mtl_rx_op &= MTL_OP_MODE_RTC_MASK; ++ if (mode <= 32) ++ mtl_rx_op |= MTL_OP_MODE_RTC_32; ++ else if (mode <= 64) ++ mtl_rx_op |= MTL_OP_MODE_RTC_64; ++ else if (mode <= 96) ++ mtl_rx_op |= MTL_OP_MODE_RTC_96; ++ else ++ mtl_rx_op |= MTL_OP_MODE_RTC_128; ++ } ++ ++ mtl_rx_op &= ~MTL_OP_MODE_RQS_MASK; ++ mtl_rx_op |= rqs << MTL_OP_MODE_RQS_SHIFT; ++ ++ /* enable flow control only if each channel gets 4 KiB or more FIFO */ ++ if (fifosz >= 4096) { ++ unsigned int rfd, rfa; ++ ++ mtl_rx_op |= MTL_OP_MODE_EHFC; ++ ++ /* Set Threshold for Activating Flow Control to min 2 frames, ++ * i.e. 1500 * 2 = 3000 bytes. ++ * ++ * Set Threshold for Deactivating Flow Control to min 1 frame, ++ * i.e. 1500 bytes. ++ */ ++ switch (fifosz) { ++ case 4096: ++ /* This violates the above formula because of FIFO size ++ * limit therefore overflow may occur in spite of this. ++ */ ++ rfd = 0x03; /* Full-2.5K */ ++ rfa = 0x01; /* Full-1.5K */ ++ break; ++ ++ case 8192: ++ rfd = 0x06; /* Full-4K */ ++ rfa = 0x0a; /* Full-6K */ ++ break; ++ ++ case 16384: ++ rfd = 0x06; /* Full-4K */ ++ rfa = 0x12; /* Full-10K */ ++ break; ++ ++ default: ++ rfd = 0x06; /* Full-4K */ ++ rfa = 0x1e; /* Full-16K */ ++ break; ++ } + +- if (txmode == SF_DMA_MODE) { ++ mtl_rx_op &= ~MTL_OP_MODE_RFD_MASK; ++ mtl_rx_op |= rfd << MTL_OP_MODE_RFD_SHIFT; ++ ++ mtl_rx_op &= ~MTL_OP_MODE_RFA_MASK; ++ mtl_rx_op |= rfa << MTL_OP_MODE_RFA_SHIFT; ++ } ++ ++ writel(mtl_rx_op, ioaddr + MTL_CHAN_RX_OP_MODE(channel)); ++ ++ /* Enable MTL RX overflow */ ++ mtl_rx_int = readl(ioaddr + MTL_CHAN_INT_CTRL(channel)); ++ writel(mtl_rx_int | MTL_RX_OVERFLOW_INT_EN, ++ ioaddr + MTL_CHAN_INT_CTRL(channel)); ++} ++ ++static void dwmac4_dma_tx_chan_op_mode(void __iomem *ioaddr, int mode, ++ u32 channel) ++{ ++ u32 mtl_tx_op = readl(ioaddr + MTL_CHAN_TX_OP_MODE(channel)); ++ ++ if (mode == SF_DMA_MODE) { + pr_debug("GMAC: enable TX store and forward mode\n"); + /* Transmit COE type 2 cannot be done in cut-through mode. */ + mtl_tx_op |= MTL_OP_MODE_TSF; + } else { +- pr_debug("GMAC: disabling TX SF (threshold %d)\n", txmode); ++ pr_debug("GMAC: disabling TX SF (threshold %d)\n", mode); + mtl_tx_op &= ~MTL_OP_MODE_TSF; + mtl_tx_op &= MTL_OP_MODE_TTC_MASK; + /* Set the transmit threshold */ +- if (txmode <= 32) ++ if (mode <= 32) + mtl_tx_op |= MTL_OP_MODE_TTC_32; +- else if (txmode <= 64) ++ else if (mode <= 64) + mtl_tx_op |= MTL_OP_MODE_TTC_64; +- else if (txmode <= 96) ++ else if (mode <= 96) + mtl_tx_op |= MTL_OP_MODE_TTC_96; +- else if (txmode <= 128) ++ else if (mode <= 128) + mtl_tx_op |= MTL_OP_MODE_TTC_128; +- else if (txmode <= 192) ++ else if (mode <= 192) + mtl_tx_op |= MTL_OP_MODE_TTC_192; +- else if (txmode <= 256) ++ else if (mode <= 256) + mtl_tx_op |= MTL_OP_MODE_TTC_256; +- else if (txmode <= 384) ++ else if (mode <= 384) + mtl_tx_op |= MTL_OP_MODE_TTC_384; + else + mtl_tx_op |= MTL_OP_MODE_TTC_512; +@@ -230,39 +313,6 @@ static void dwmac4_dma_chan_op_mode(void + */ + mtl_tx_op |= MTL_OP_MODE_TXQEN | MTL_OP_MODE_TQS_MASK; + writel(mtl_tx_op, ioaddr + MTL_CHAN_TX_OP_MODE(channel)); +- +- mtl_rx_op = readl(ioaddr + MTL_CHAN_RX_OP_MODE(channel)); +- +- if (rxmode == SF_DMA_MODE) { +- pr_debug("GMAC: enable RX store and forward mode\n"); +- mtl_rx_op |= MTL_OP_MODE_RSF; +- } else { +- pr_debug("GMAC: disable RX SF mode (threshold %d)\n", rxmode); +- mtl_rx_op &= ~MTL_OP_MODE_RSF; +- mtl_rx_op &= MTL_OP_MODE_RTC_MASK; +- if (rxmode <= 32) +- mtl_rx_op |= MTL_OP_MODE_RTC_32; +- else if (rxmode <= 64) +- mtl_rx_op |= MTL_OP_MODE_RTC_64; +- else if (rxmode <= 96) +- mtl_rx_op |= MTL_OP_MODE_RTC_96; +- else +- mtl_rx_op |= MTL_OP_MODE_RTC_128; +- } +- +- writel(mtl_rx_op, ioaddr + MTL_CHAN_RX_OP_MODE(channel)); +- +- /* Enable MTL RX overflow */ +- mtl_rx_int = readl(ioaddr + MTL_CHAN_INT_CTRL(channel)); +- writel(mtl_rx_int | MTL_RX_OVERFLOW_INT_EN, +- ioaddr + MTL_CHAN_INT_CTRL(channel)); +-} +- +-static void dwmac4_dma_operation_mode(void __iomem *ioaddr, int txmode, +- int rxmode, int rxfifosz) +-{ +- /* Only Channel 0 is actually configured and used */ +- dwmac4_dma_chan_op_mode(ioaddr, txmode, rxmode, 0); + } + + static void dwmac4_get_hw_feature(void __iomem *ioaddr, +@@ -294,6 +344,11 @@ static void dwmac4_get_hw_feature(void _ + hw_cap = readl(ioaddr + GMAC_HW_FEATURE1); + dma_cap->av = (hw_cap & GMAC_HW_FEAT_AVSEL) >> 20; + dma_cap->tsoen = (hw_cap & GMAC_HW_TSOEN) >> 18; ++ /* RX and TX FIFO sizes are encoded as log2(n / 128). Undo that by ++ * shifting and store the sizes in bytes. ++ */ ++ dma_cap->tx_fifo_size = 128 << ((hw_cap & GMAC_HW_TXFIFOSIZE) >> 6); ++ dma_cap->rx_fifo_size = 128 << ((hw_cap & GMAC_HW_RXFIFOSIZE) >> 0); + /* MAC HW feature2 */ + hw_cap = readl(ioaddr + GMAC_HW_FEATURE2); + /* TX and RX number of channels */ +@@ -332,9 +387,13 @@ static void dwmac4_enable_tso(void __iom + const struct stmmac_dma_ops dwmac4_dma_ops = { + .reset = dwmac4_dma_reset, + .init = dwmac4_dma_init, ++ .init_chan = dwmac4_dma_init_channel, ++ .init_rx_chan = dwmac4_dma_init_rx_chan, ++ .init_tx_chan = dwmac4_dma_init_tx_chan, + .axi = dwmac4_dma_axi, + .dump_regs = dwmac4_dump_dma_regs, +- .dma_mode = dwmac4_dma_operation_mode, ++ .dma_rx_mode = dwmac4_dma_rx_chan_op_mode, ++ .dma_tx_mode = dwmac4_dma_tx_chan_op_mode, + .enable_dma_irq = dwmac4_enable_dma_irq, + .disable_dma_irq = dwmac4_disable_dma_irq, + .start_tx = dwmac4_dma_start_tx, +@@ -354,9 +413,13 @@ const struct stmmac_dma_ops dwmac4_dma_o + const struct stmmac_dma_ops dwmac410_dma_ops = { + .reset = dwmac4_dma_reset, + .init = dwmac4_dma_init, ++ .init_chan = dwmac4_dma_init_channel, ++ .init_rx_chan = dwmac4_dma_init_rx_chan, ++ .init_tx_chan = dwmac4_dma_init_tx_chan, + .axi = dwmac4_dma_axi, + .dump_regs = dwmac4_dump_dma_regs, +- .dma_mode = dwmac4_dma_operation_mode, ++ .dma_rx_mode = dwmac4_dma_rx_chan_op_mode, ++ .dma_tx_mode = dwmac4_dma_tx_chan_op_mode, + .enable_dma_irq = dwmac410_enable_dma_irq, + .disable_dma_irq = dwmac4_disable_dma_irq, + .start_tx = dwmac4_dma_start_tx, +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.h ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.h +@@ -185,17 +185,17 @@ + + int dwmac4_dma_reset(void __iomem *ioaddr); + void dwmac4_enable_dma_transmission(void __iomem *ioaddr, u32 tail_ptr); +-void dwmac4_enable_dma_irq(void __iomem *ioaddr); +-void dwmac410_enable_dma_irq(void __iomem *ioaddr); +-void dwmac4_disable_dma_irq(void __iomem *ioaddr); +-void dwmac4_dma_start_tx(void __iomem *ioaddr); +-void dwmac4_dma_stop_tx(void __iomem *ioaddr); +-void dwmac4_dma_start_rx(void __iomem *ioaddr); +-void dwmac4_dma_stop_rx(void __iomem *ioaddr); ++void dwmac4_enable_dma_irq(void __iomem *ioaddr, u32 chan); ++void dwmac410_enable_dma_irq(void __iomem *ioaddr, u32 chan); ++void dwmac4_disable_dma_irq(void __iomem *ioaddr, u32 chan); ++void dwmac4_dma_start_tx(void __iomem *ioaddr, u32 chan); ++void dwmac4_dma_stop_tx(void __iomem *ioaddr, u32 chan); ++void dwmac4_dma_start_rx(void __iomem *ioaddr, u32 chan); ++void dwmac4_dma_stop_rx(void __iomem *ioaddr, u32 chan); + int dwmac4_dma_interrupt(void __iomem *ioaddr, +- struct stmmac_extra_stats *x); +-void dwmac4_set_rx_ring_len(void __iomem *ioaddr, u32 len); +-void dwmac4_set_tx_ring_len(void __iomem *ioaddr, u32 len); ++ struct stmmac_extra_stats *x, u32 chan); ++void dwmac4_set_rx_ring_len(void __iomem *ioaddr, u32 len, u32 chan); ++void dwmac4_set_tx_ring_len(void __iomem *ioaddr, u32 len, u32 chan); + void dwmac4_set_rx_tail_ptr(void __iomem *ioaddr, u32 tail_ptr, u32 chan); + void dwmac4_set_tx_tail_ptr(void __iomem *ioaddr, u32 tail_ptr, u32 chan); + +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_lib.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_lib.c +@@ -37,96 +37,96 @@ int dwmac4_dma_reset(void __iomem *ioadd + + void dwmac4_set_rx_tail_ptr(void __iomem *ioaddr, u32 tail_ptr, u32 chan) + { +- writel(tail_ptr, ioaddr + DMA_CHAN_RX_END_ADDR(0)); ++ writel(tail_ptr, ioaddr + DMA_CHAN_RX_END_ADDR(chan)); + } + + void dwmac4_set_tx_tail_ptr(void __iomem *ioaddr, u32 tail_ptr, u32 chan) + { +- writel(tail_ptr, ioaddr + DMA_CHAN_TX_END_ADDR(0)); ++ writel(tail_ptr, ioaddr + DMA_CHAN_TX_END_ADDR(chan)); + } + +-void dwmac4_dma_start_tx(void __iomem *ioaddr) ++void dwmac4_dma_start_tx(void __iomem *ioaddr, u32 chan) + { +- u32 value = readl(ioaddr + DMA_CHAN_TX_CONTROL(STMMAC_CHAN0)); ++ u32 value = readl(ioaddr + DMA_CHAN_TX_CONTROL(chan)); + + value |= DMA_CONTROL_ST; +- writel(value, ioaddr + DMA_CHAN_TX_CONTROL(STMMAC_CHAN0)); ++ writel(value, ioaddr + DMA_CHAN_TX_CONTROL(chan)); + + value = readl(ioaddr + GMAC_CONFIG); + value |= GMAC_CONFIG_TE; + writel(value, ioaddr + GMAC_CONFIG); + } + +-void dwmac4_dma_stop_tx(void __iomem *ioaddr) ++void dwmac4_dma_stop_tx(void __iomem *ioaddr, u32 chan) + { +- u32 value = readl(ioaddr + DMA_CHAN_TX_CONTROL(STMMAC_CHAN0)); ++ u32 value = readl(ioaddr + DMA_CHAN_TX_CONTROL(chan)); + + value &= ~DMA_CONTROL_ST; +- writel(value, ioaddr + DMA_CHAN_TX_CONTROL(STMMAC_CHAN0)); ++ writel(value, ioaddr + DMA_CHAN_TX_CONTROL(chan)); + + value = readl(ioaddr + GMAC_CONFIG); + value &= ~GMAC_CONFIG_TE; + writel(value, ioaddr + GMAC_CONFIG); + } + +-void dwmac4_dma_start_rx(void __iomem *ioaddr) ++void dwmac4_dma_start_rx(void __iomem *ioaddr, u32 chan) + { +- u32 value = readl(ioaddr + DMA_CHAN_RX_CONTROL(STMMAC_CHAN0)); ++ u32 value = readl(ioaddr + DMA_CHAN_RX_CONTROL(chan)); + + value |= DMA_CONTROL_SR; + +- writel(value, ioaddr + DMA_CHAN_RX_CONTROL(STMMAC_CHAN0)); ++ writel(value, ioaddr + DMA_CHAN_RX_CONTROL(chan)); + + value = readl(ioaddr + GMAC_CONFIG); + value |= GMAC_CONFIG_RE; + writel(value, ioaddr + GMAC_CONFIG); + } + +-void dwmac4_dma_stop_rx(void __iomem *ioaddr) ++void dwmac4_dma_stop_rx(void __iomem *ioaddr, u32 chan) + { +- u32 value = readl(ioaddr + DMA_CHAN_RX_CONTROL(STMMAC_CHAN0)); ++ u32 value = readl(ioaddr + DMA_CHAN_RX_CONTROL(chan)); + + value &= ~DMA_CONTROL_SR; +- writel(value, ioaddr + DMA_CHAN_RX_CONTROL(STMMAC_CHAN0)); ++ writel(value, ioaddr + DMA_CHAN_RX_CONTROL(chan)); + + value = readl(ioaddr + GMAC_CONFIG); + value &= ~GMAC_CONFIG_RE; + writel(value, ioaddr + GMAC_CONFIG); + } + +-void dwmac4_set_tx_ring_len(void __iomem *ioaddr, u32 len) ++void dwmac4_set_tx_ring_len(void __iomem *ioaddr, u32 len, u32 chan) + { +- writel(len, ioaddr + DMA_CHAN_TX_RING_LEN(STMMAC_CHAN0)); ++ writel(len, ioaddr + DMA_CHAN_TX_RING_LEN(chan)); + } + +-void dwmac4_set_rx_ring_len(void __iomem *ioaddr, u32 len) ++void dwmac4_set_rx_ring_len(void __iomem *ioaddr, u32 len, u32 chan) + { +- writel(len, ioaddr + DMA_CHAN_RX_RING_LEN(STMMAC_CHAN0)); ++ writel(len, ioaddr + DMA_CHAN_RX_RING_LEN(chan)); + } + +-void dwmac4_enable_dma_irq(void __iomem *ioaddr) ++void dwmac4_enable_dma_irq(void __iomem *ioaddr, u32 chan) + { + writel(DMA_CHAN_INTR_DEFAULT_MASK, ioaddr + +- DMA_CHAN_INTR_ENA(STMMAC_CHAN0)); ++ DMA_CHAN_INTR_ENA(chan)); + } + +-void dwmac410_enable_dma_irq(void __iomem *ioaddr) ++void dwmac410_enable_dma_irq(void __iomem *ioaddr, u32 chan) + { + writel(DMA_CHAN_INTR_DEFAULT_MASK_4_10, +- ioaddr + DMA_CHAN_INTR_ENA(STMMAC_CHAN0)); ++ ioaddr + DMA_CHAN_INTR_ENA(chan)); + } + +-void dwmac4_disable_dma_irq(void __iomem *ioaddr) ++void dwmac4_disable_dma_irq(void __iomem *ioaddr, u32 chan) + { +- writel(0, ioaddr + DMA_CHAN_INTR_ENA(STMMAC_CHAN0)); ++ writel(0, ioaddr + DMA_CHAN_INTR_ENA(chan)); + } + + int dwmac4_dma_interrupt(void __iomem *ioaddr, +- struct stmmac_extra_stats *x) ++ struct stmmac_extra_stats *x, u32 chan) + { + int ret = 0; + +- u32 intr_status = readl(ioaddr + DMA_CHAN_STATUS(0)); ++ u32 intr_status = readl(ioaddr + DMA_CHAN_STATUS(chan)); + + /* ABNORMAL interrupts */ + if (unlikely(intr_status & DMA_CHAN_STATUS_AIS)) { +@@ -153,7 +153,7 @@ int dwmac4_dma_interrupt(void __iomem *i + if (likely(intr_status & DMA_CHAN_STATUS_RI)) { + u32 value; + +- value = readl(ioaddr + DMA_CHAN_INTR_ENA(STMMAC_CHAN0)); ++ value = readl(ioaddr + DMA_CHAN_INTR_ENA(chan)); + /* to schedule NAPI on real RIE event. */ + if (likely(value & DMA_CHAN_INTR_ENA_RIE)) { + x->rx_normal_irq_n++; +@@ -172,7 +172,7 @@ int dwmac4_dma_interrupt(void __iomem *i + * status [21-0] expect reserved bits [5-3] + */ + writel((intr_status & 0x3fffc7), +- ioaddr + DMA_CHAN_STATUS(STMMAC_CHAN0)); ++ ioaddr + DMA_CHAN_STATUS(chan)); + + return ret; + } +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h +@@ -137,13 +137,14 @@ + #define DMA_CONTROL_FTF 0x00100000 /* Flush transmit FIFO */ + + void dwmac_enable_dma_transmission(void __iomem *ioaddr); +-void dwmac_enable_dma_irq(void __iomem *ioaddr); +-void dwmac_disable_dma_irq(void __iomem *ioaddr); +-void dwmac_dma_start_tx(void __iomem *ioaddr); +-void dwmac_dma_stop_tx(void __iomem *ioaddr); +-void dwmac_dma_start_rx(void __iomem *ioaddr); +-void dwmac_dma_stop_rx(void __iomem *ioaddr); +-int dwmac_dma_interrupt(void __iomem *ioaddr, struct stmmac_extra_stats *x); ++void dwmac_enable_dma_irq(void __iomem *ioaddr, u32 chan); ++void dwmac_disable_dma_irq(void __iomem *ioaddr, u32 chan); ++void dwmac_dma_start_tx(void __iomem *ioaddr, u32 chan); ++void dwmac_dma_stop_tx(void __iomem *ioaddr, u32 chan); ++void dwmac_dma_start_rx(void __iomem *ioaddr, u32 chan); ++void dwmac_dma_stop_rx(void __iomem *ioaddr, u32 chan); ++int dwmac_dma_interrupt(void __iomem *ioaddr, struct stmmac_extra_stats *x, ++ u32 chan); + int dwmac_dma_reset(void __iomem *ioaddr); + + #endif /* __DWMAC_DMA_H__ */ +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c +@@ -47,38 +47,38 @@ void dwmac_enable_dma_transmission(void + writel(1, ioaddr + DMA_XMT_POLL_DEMAND); + } + +-void dwmac_enable_dma_irq(void __iomem *ioaddr) ++void dwmac_enable_dma_irq(void __iomem *ioaddr, u32 chan) + { + writel(DMA_INTR_DEFAULT_MASK, ioaddr + DMA_INTR_ENA); + } + +-void dwmac_disable_dma_irq(void __iomem *ioaddr) ++void dwmac_disable_dma_irq(void __iomem *ioaddr, u32 chan) + { + writel(0, ioaddr + DMA_INTR_ENA); + } + +-void dwmac_dma_start_tx(void __iomem *ioaddr) ++void dwmac_dma_start_tx(void __iomem *ioaddr, u32 chan) + { + u32 value = readl(ioaddr + DMA_CONTROL); + value |= DMA_CONTROL_ST; + writel(value, ioaddr + DMA_CONTROL); + } + +-void dwmac_dma_stop_tx(void __iomem *ioaddr) ++void dwmac_dma_stop_tx(void __iomem *ioaddr, u32 chan) + { + u32 value = readl(ioaddr + DMA_CONTROL); + value &= ~DMA_CONTROL_ST; + writel(value, ioaddr + DMA_CONTROL); + } + +-void dwmac_dma_start_rx(void __iomem *ioaddr) ++void dwmac_dma_start_rx(void __iomem *ioaddr, u32 chan) + { + u32 value = readl(ioaddr + DMA_CONTROL); + value |= DMA_CONTROL_SR; + writel(value, ioaddr + DMA_CONTROL); + } + +-void dwmac_dma_stop_rx(void __iomem *ioaddr) ++void dwmac_dma_stop_rx(void __iomem *ioaddr, u32 chan) + { + u32 value = readl(ioaddr + DMA_CONTROL); + value &= ~DMA_CONTROL_SR; +@@ -156,7 +156,7 @@ static void show_rx_process_state(unsign + #endif + + int dwmac_dma_interrupt(void __iomem *ioaddr, +- struct stmmac_extra_stats *x) ++ struct stmmac_extra_stats *x, u32 chan) + { + int ret = 0; + /* read the status register (CSR5) */ +--- a/drivers/net/ethernet/stmicro/stmmac/enh_desc.c ++++ b/drivers/net/ethernet/stmicro/stmmac/enh_desc.c +@@ -315,7 +315,7 @@ static void enh_desc_release_tx_desc(str + + static void enh_desc_prepare_tx_desc(struct dma_desc *p, int is_fs, int len, + bool csum_flag, int mode, bool tx_own, +- bool ls) ++ bool ls, unsigned int tot_pkt_len) + { + unsigned int tdes0 = le32_to_cpu(p->des0); + +--- a/drivers/net/ethernet/stmicro/stmmac/norm_desc.c ++++ b/drivers/net/ethernet/stmicro/stmmac/norm_desc.c +@@ -191,7 +191,7 @@ static void ndesc_release_tx_desc(struct + + static void ndesc_prepare_tx_desc(struct dma_desc *p, int is_fs, int len, + bool csum_flag, int mode, bool tx_own, +- bool ls) ++ bool ls, unsigned int tot_pkt_len) + { + unsigned int tdes1 = le32_to_cpu(p->des1); + +--- a/drivers/net/ethernet/stmicro/stmmac/ring_mode.c ++++ b/drivers/net/ethernet/stmicro/stmmac/ring_mode.c +@@ -26,16 +26,17 @@ + + static int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum) + { +- struct stmmac_priv *priv = (struct stmmac_priv *)p; +- unsigned int entry = priv->cur_tx; +- struct dma_desc *desc; ++ struct stmmac_tx_queue *tx_q = (struct stmmac_tx_queue *)p; + unsigned int nopaged_len = skb_headlen(skb); ++ struct stmmac_priv *priv = tx_q->priv_data; ++ unsigned int entry = tx_q->cur_tx; + unsigned int bmax, len, des2; ++ struct dma_desc *desc; + + if (priv->extend_desc) +- desc = (struct dma_desc *)(priv->dma_etx + entry); ++ desc = (struct dma_desc *)(tx_q->dma_etx + entry); + else +- desc = priv->dma_tx + entry; ++ desc = tx_q->dma_tx + entry; + + if (priv->plat->enh_desc) + bmax = BUF_SIZE_8KiB; +@@ -52,48 +53,51 @@ static int stmmac_jumbo_frm(void *p, str + if (dma_mapping_error(priv->device, des2)) + return -1; + +- priv->tx_skbuff_dma[entry].buf = des2; +- priv->tx_skbuff_dma[entry].len = bmax; +- priv->tx_skbuff_dma[entry].is_jumbo = true; ++ tx_q->tx_skbuff_dma[entry].buf = des2; ++ tx_q->tx_skbuff_dma[entry].len = bmax; ++ tx_q->tx_skbuff_dma[entry].is_jumbo = true; + + desc->des3 = cpu_to_le32(des2 + BUF_SIZE_4KiB); + priv->hw->desc->prepare_tx_desc(desc, 1, bmax, csum, +- STMMAC_RING_MODE, 0, false); +- priv->tx_skbuff[entry] = NULL; ++ STMMAC_RING_MODE, 0, ++ false, skb->len); ++ tx_q->tx_skbuff[entry] = NULL; + entry = STMMAC_GET_ENTRY(entry, DMA_TX_SIZE); + + if (priv->extend_desc) +- desc = (struct dma_desc *)(priv->dma_etx + entry); ++ desc = (struct dma_desc *)(tx_q->dma_etx + entry); + else +- desc = priv->dma_tx + entry; ++ desc = tx_q->dma_tx + entry; + + des2 = dma_map_single(priv->device, skb->data + bmax, len, + DMA_TO_DEVICE); + desc->des2 = cpu_to_le32(des2); + if (dma_mapping_error(priv->device, des2)) + return -1; +- priv->tx_skbuff_dma[entry].buf = des2; +- priv->tx_skbuff_dma[entry].len = len; +- priv->tx_skbuff_dma[entry].is_jumbo = true; ++ tx_q->tx_skbuff_dma[entry].buf = des2; ++ tx_q->tx_skbuff_dma[entry].len = len; ++ tx_q->tx_skbuff_dma[entry].is_jumbo = true; + + desc->des3 = cpu_to_le32(des2 + BUF_SIZE_4KiB); + priv->hw->desc->prepare_tx_desc(desc, 0, len, csum, +- STMMAC_RING_MODE, 1, true); ++ STMMAC_RING_MODE, 1, ++ true, skb->len); + } else { + des2 = dma_map_single(priv->device, skb->data, + nopaged_len, DMA_TO_DEVICE); + desc->des2 = cpu_to_le32(des2); + if (dma_mapping_error(priv->device, des2)) + return -1; +- priv->tx_skbuff_dma[entry].buf = des2; +- priv->tx_skbuff_dma[entry].len = nopaged_len; +- priv->tx_skbuff_dma[entry].is_jumbo = true; ++ tx_q->tx_skbuff_dma[entry].buf = des2; ++ tx_q->tx_skbuff_dma[entry].len = nopaged_len; ++ tx_q->tx_skbuff_dma[entry].is_jumbo = true; + desc->des3 = cpu_to_le32(des2 + BUF_SIZE_4KiB); + priv->hw->desc->prepare_tx_desc(desc, 1, nopaged_len, csum, +- STMMAC_RING_MODE, 0, true); ++ STMMAC_RING_MODE, 0, ++ true, skb->len); + } + +- priv->cur_tx = entry; ++ tx_q->cur_tx = entry; + + return entry; + } +@@ -125,12 +129,13 @@ static void stmmac_init_desc3(struct dma + + static void stmmac_clean_desc3(void *priv_ptr, struct dma_desc *p) + { +- struct stmmac_priv *priv = (struct stmmac_priv *)priv_ptr; +- unsigned int entry = priv->dirty_tx; ++ struct stmmac_tx_queue *tx_q = (struct stmmac_tx_queue *)priv_ptr; ++ struct stmmac_priv *priv = tx_q->priv_data; ++ unsigned int entry = tx_q->dirty_tx; + + /* des3 is only used for jumbo frames tx or time stamping */ +- if (unlikely(priv->tx_skbuff_dma[entry].is_jumbo || +- (priv->tx_skbuff_dma[entry].last_segment && ++ if (unlikely(tx_q->tx_skbuff_dma[entry].is_jumbo || ++ (tx_q->tx_skbuff_dma[entry].last_segment && + !priv->extend_desc && priv->hwts_tx_en))) + p->des3 = 0; + } +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h +@@ -46,38 +46,51 @@ struct stmmac_tx_info { + bool is_jumbo; + }; + +-struct stmmac_priv { +- /* Frequently used values are kept adjacent for cache effect */ ++/* Frequently used values are kept adjacent for cache effect */ ++struct stmmac_tx_queue { ++ u32 queue_index; ++ struct stmmac_priv *priv_data; + struct dma_extended_desc *dma_etx ____cacheline_aligned_in_smp; + struct dma_desc *dma_tx; + struct sk_buff **tx_skbuff; ++ struct stmmac_tx_info *tx_skbuff_dma; + unsigned int cur_tx; + unsigned int dirty_tx; ++ dma_addr_t dma_tx_phy; ++ u32 tx_tail_addr; ++}; ++ ++struct stmmac_rx_queue { ++ u32 queue_index; ++ struct stmmac_priv *priv_data; ++ struct dma_extended_desc *dma_erx; ++ struct dma_desc *dma_rx ____cacheline_aligned_in_smp; ++ struct sk_buff **rx_skbuff; ++ dma_addr_t *rx_skbuff_dma; ++ unsigned int cur_rx; ++ unsigned int dirty_rx; ++ u32 rx_zeroc_thresh; ++ dma_addr_t dma_rx_phy; ++ u32 rx_tail_addr; ++ struct napi_struct napi ____cacheline_aligned_in_smp; ++}; ++ ++struct stmmac_priv { ++ /* Frequently used values are kept adjacent for cache effect */ + u32 tx_count_frames; + u32 tx_coal_frames; + u32 tx_coal_timer; +- struct stmmac_tx_info *tx_skbuff_dma; +- dma_addr_t dma_tx_phy; ++ + int tx_coalesce; + int hwts_tx_en; + bool tx_path_in_lpi_mode; + struct timer_list txtimer; + bool tso; + +- struct dma_desc *dma_rx ____cacheline_aligned_in_smp; +- struct dma_extended_desc *dma_erx; +- struct sk_buff **rx_skbuff; +- unsigned int cur_rx; +- unsigned int dirty_rx; + unsigned int dma_buf_sz; + unsigned int rx_copybreak; +- unsigned int rx_zeroc_thresh; + u32 rx_riwt; + int hwts_rx_en; +- dma_addr_t *rx_skbuff_dma; +- dma_addr_t dma_rx_phy; +- +- struct napi_struct napi ____cacheline_aligned_in_smp; + + void __iomem *ioaddr; + struct net_device *dev; +@@ -85,6 +98,12 @@ struct stmmac_priv { + struct mac_device_info *hw; + spinlock_t lock; + ++ /* RX Queue */ ++ struct stmmac_rx_queue rx_queue[MTL_MAX_RX_QUEUES]; ++ ++ /* TX Queue */ ++ struct stmmac_tx_queue tx_queue[MTL_MAX_TX_QUEUES]; ++ + int oldlink; + int speed; + int oldduplex; +@@ -119,8 +138,6 @@ struct stmmac_priv { + spinlock_t ptp_lock; + void __iomem *mmcaddr; + void __iomem *ptpaddr; +- u32 rx_tail_addr; +- u32 tx_tail_addr; + u32 mss; + + #ifdef CONFIG_DEBUG_FS +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c +@@ -481,6 +481,7 @@ stmmac_set_pauseparam(struct net_device + struct ethtool_pauseparam *pause) + { + struct stmmac_priv *priv = netdev_priv(netdev); ++ u32 tx_cnt = priv->plat->tx_queues_to_use; + struct phy_device *phy = netdev->phydev; + int new_pause = FLOW_OFF; + +@@ -511,7 +512,7 @@ stmmac_set_pauseparam(struct net_device + } + + priv->hw->mac->flow_ctrl(priv->hw, phy->duplex, priv->flow_ctrl, +- priv->pause); ++ priv->pause, tx_cnt); + return 0; + } + +@@ -519,6 +520,8 @@ static void stmmac_get_ethtool_stats(str + struct ethtool_stats *dummy, u64 *data) + { + struct stmmac_priv *priv = netdev_priv(dev); ++ u32 rx_queues_count = priv->plat->rx_queues_to_use; ++ u32 tx_queues_count = priv->plat->tx_queues_to_use; + int i, j = 0; + + /* Update the DMA HW counters for dwmac10/100 */ +@@ -549,7 +552,8 @@ static void stmmac_get_ethtool_stats(str + if ((priv->hw->mac->debug) && + (priv->synopsys_id >= DWMAC_CORE_3_50)) + priv->hw->mac->debug(priv->ioaddr, +- (void *)&priv->xstats); ++ (void *)&priv->xstats, ++ rx_queues_count, tx_queues_count); + } + for (i = 0; i < STMMAC_STATS_LEN; i++) { + char *p = (char *)priv + stmmac_gstrings_stats[i].stat_offset; +@@ -726,6 +730,7 @@ static int stmmac_set_coalesce(struct ne + struct ethtool_coalesce *ec) + { + struct stmmac_priv *priv = netdev_priv(dev); ++ u32 rx_cnt = priv->plat->rx_queues_to_use; + unsigned int rx_riwt; + + /* Check not supported parameters */ +@@ -764,7 +769,7 @@ static int stmmac_set_coalesce(struct ne + priv->tx_coal_frames = ec->tx_max_coalesced_frames; + priv->tx_coal_timer = ec->tx_coalesce_usecs; + priv->rx_riwt = rx_riwt; +- priv->hw->dma->rx_watchdog(priv->ioaddr, priv->rx_riwt); ++ priv->hw->dma->rx_watchdog(priv->ioaddr, priv->rx_riwt, rx_cnt); + + return 0; + } +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +@@ -139,6 +139,64 @@ static void stmmac_verify_args(void) + } + + /** ++ * stmmac_disable_all_queues - Disable all queues ++ * @priv: driver private structure ++ */ ++static void stmmac_disable_all_queues(struct stmmac_priv *priv) ++{ ++ u32 rx_queues_cnt = priv->plat->rx_queues_to_use; ++ u32 queue; ++ ++ for (queue = 0; queue < rx_queues_cnt; queue++) { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; ++ ++ napi_disable(&rx_q->napi); ++ } ++} ++ ++/** ++ * stmmac_enable_all_queues - Enable all queues ++ * @priv: driver private structure ++ */ ++static void stmmac_enable_all_queues(struct stmmac_priv *priv) ++{ ++ u32 rx_queues_cnt = priv->plat->rx_queues_to_use; ++ u32 queue; ++ ++ for (queue = 0; queue < rx_queues_cnt; queue++) { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; ++ ++ napi_enable(&rx_q->napi); ++ } ++} ++ ++/** ++ * stmmac_stop_all_queues - Stop all queues ++ * @priv: driver private structure ++ */ ++static void stmmac_stop_all_queues(struct stmmac_priv *priv) ++{ ++ u32 tx_queues_cnt = priv->plat->tx_queues_to_use; ++ u32 queue; ++ ++ for (queue = 0; queue < tx_queues_cnt; queue++) ++ netif_tx_stop_queue(netdev_get_tx_queue(priv->dev, queue)); ++} ++ ++/** ++ * stmmac_start_all_queues - Start all queues ++ * @priv: driver private structure ++ */ ++static void stmmac_start_all_queues(struct stmmac_priv *priv) ++{ ++ u32 tx_queues_cnt = priv->plat->tx_queues_to_use; ++ u32 queue; ++ ++ for (queue = 0; queue < tx_queues_cnt; queue++) ++ netif_tx_start_queue(netdev_get_tx_queue(priv->dev, queue)); ++} ++ ++/** + * stmmac_clk_csr_set - dynamically set the MDC clock + * @priv: driver private structure + * Description: this is to dynamically set the MDC clock according to the csr +@@ -185,26 +243,33 @@ static void print_pkt(unsigned char *buf + print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, buf, len); + } + +-static inline u32 stmmac_tx_avail(struct stmmac_priv *priv) ++static inline u32 stmmac_tx_avail(struct stmmac_priv *priv, u32 queue) + { ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue]; + u32 avail; + +- if (priv->dirty_tx > priv->cur_tx) +- avail = priv->dirty_tx - priv->cur_tx - 1; ++ if (tx_q->dirty_tx > tx_q->cur_tx) ++ avail = tx_q->dirty_tx - tx_q->cur_tx - 1; + else +- avail = DMA_TX_SIZE - priv->cur_tx + priv->dirty_tx - 1; ++ avail = DMA_TX_SIZE - tx_q->cur_tx + tx_q->dirty_tx - 1; + + return avail; + } + +-static inline u32 stmmac_rx_dirty(struct stmmac_priv *priv) ++/** ++ * stmmac_rx_dirty - Get RX queue dirty ++ * @priv: driver private structure ++ * @queue: RX queue index ++ */ ++static inline u32 stmmac_rx_dirty(struct stmmac_priv *priv, u32 queue) + { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; + u32 dirty; + +- if (priv->dirty_rx <= priv->cur_rx) +- dirty = priv->cur_rx - priv->dirty_rx; ++ if (rx_q->dirty_rx <= rx_q->cur_rx) ++ dirty = rx_q->cur_rx - rx_q->dirty_rx; + else +- dirty = DMA_RX_SIZE - priv->dirty_rx + priv->cur_rx; ++ dirty = DMA_RX_SIZE - rx_q->dirty_rx + rx_q->cur_rx; + + return dirty; + } +@@ -232,9 +297,19 @@ static inline void stmmac_hw_fix_mac_spe + */ + static void stmmac_enable_eee_mode(struct stmmac_priv *priv) + { ++ u32 tx_cnt = priv->plat->tx_queues_to_use; ++ u32 queue; ++ ++ /* check if all TX queues have the work finished */ ++ for (queue = 0; queue < tx_cnt; queue++) { ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue]; ++ ++ if (tx_q->dirty_tx != tx_q->cur_tx) ++ return; /* still unfinished work */ ++ } ++ + /* Check and enter in LPI mode */ +- if ((priv->dirty_tx == priv->cur_tx) && +- (priv->tx_path_in_lpi_mode == false)) ++ if (!priv->tx_path_in_lpi_mode) + priv->hw->mac->set_eee_mode(priv->hw, + priv->plat->en_tx_lpi_clockgating); + } +@@ -359,14 +434,14 @@ static void stmmac_get_tx_hwtstamp(struc + return; + + /* check tx tstamp status */ +- if (!priv->hw->desc->get_tx_timestamp_status(p)) { ++ if (priv->hw->desc->get_tx_timestamp_status(p)) { + /* get the valid tstamp */ + ns = priv->hw->desc->get_timestamp(p, priv->adv_ts); + + memset(&shhwtstamp, 0, sizeof(struct skb_shared_hwtstamps)); + shhwtstamp.hwtstamp = ns_to_ktime(ns); + +- netdev_info(priv->dev, "get valid TX hw timestamp %llu\n", ns); ++ netdev_dbg(priv->dev, "get valid TX hw timestamp %llu\n", ns); + /* pass tstamp to stack */ + skb_tstamp_tx(skb, &shhwtstamp); + } +@@ -393,19 +468,19 @@ static void stmmac_get_rx_hwtstamp(struc + return; + + /* Check if timestamp is available */ +- if (!priv->hw->desc->get_rx_timestamp_status(p, priv->adv_ts)) { ++ if (priv->hw->desc->get_rx_timestamp_status(p, priv->adv_ts)) { + /* For GMAC4, the valid timestamp is from CTX next desc. */ + if (priv->plat->has_gmac4) + ns = priv->hw->desc->get_timestamp(np, priv->adv_ts); + else + ns = priv->hw->desc->get_timestamp(p, priv->adv_ts); + +- netdev_info(priv->dev, "get valid RX hw timestamp %llu\n", ns); ++ netdev_dbg(priv->dev, "get valid RX hw timestamp %llu\n", ns); + shhwtstamp = skb_hwtstamps(skb); + memset(shhwtstamp, 0, sizeof(struct skb_shared_hwtstamps)); + shhwtstamp->hwtstamp = ns_to_ktime(ns); + } else { +- netdev_err(priv->dev, "cannot get RX hw timestamp\n"); ++ netdev_dbg(priv->dev, "cannot get RX hw timestamp\n"); + } + } + +@@ -471,7 +546,10 @@ static int stmmac_hwtstamp_ioctl(struct + /* PTP v1, UDP, any kind of event packet */ + config.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT; + /* take time stamp for all event messages */ +- snap_type_sel = PTP_TCR_SNAPTYPSEL_1; ++ if (priv->plat->has_gmac4) ++ snap_type_sel = PTP_GMAC4_TCR_SNAPTYPSEL_1; ++ else ++ snap_type_sel = PTP_TCR_SNAPTYPSEL_1; + + ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA; + ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA; +@@ -503,7 +581,10 @@ static int stmmac_hwtstamp_ioctl(struct + config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT; + ptp_v2 = PTP_TCR_TSVER2ENA; + /* take time stamp for all event messages */ +- snap_type_sel = PTP_TCR_SNAPTYPSEL_1; ++ if (priv->plat->has_gmac4) ++ snap_type_sel = PTP_GMAC4_TCR_SNAPTYPSEL_1; ++ else ++ snap_type_sel = PTP_TCR_SNAPTYPSEL_1; + + ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA; + ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA; +@@ -537,7 +618,10 @@ static int stmmac_hwtstamp_ioctl(struct + config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; + ptp_v2 = PTP_TCR_TSVER2ENA; + /* take time stamp for all event messages */ +- snap_type_sel = PTP_TCR_SNAPTYPSEL_1; ++ if (priv->plat->has_gmac4) ++ snap_type_sel = PTP_GMAC4_TCR_SNAPTYPSEL_1; ++ else ++ snap_type_sel = PTP_TCR_SNAPTYPSEL_1; + + ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA; + ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA; +@@ -673,6 +757,19 @@ static void stmmac_release_ptp(struct st + } + + /** ++ * stmmac_mac_flow_ctrl - Configure flow control in all queues ++ * @priv: driver private structure ++ * Description: It is used for configuring the flow control in all queues ++ */ ++static void stmmac_mac_flow_ctrl(struct stmmac_priv *priv, u32 duplex) ++{ ++ u32 tx_cnt = priv->plat->tx_queues_to_use; ++ ++ priv->hw->mac->flow_ctrl(priv->hw, duplex, priv->flow_ctrl, ++ priv->pause, tx_cnt); ++} ++ ++/** + * stmmac_adjust_link - adjusts the link parameters + * @dev: net device structure + * Description: this is the helper called by the physical abstraction layer +@@ -687,7 +784,6 @@ static void stmmac_adjust_link(struct ne + struct phy_device *phydev = dev->phydev; + unsigned long flags; + int new_state = 0; +- unsigned int fc = priv->flow_ctrl, pause_time = priv->pause; + + if (!phydev) + return; +@@ -709,8 +805,7 @@ static void stmmac_adjust_link(struct ne + } + /* Flow Control operation */ + if (phydev->pause) +- priv->hw->mac->flow_ctrl(priv->hw, phydev->duplex, +- fc, pause_time); ++ stmmac_mac_flow_ctrl(priv, phydev->duplex); + + if (phydev->speed != priv->speed) { + new_state = 1; +@@ -878,22 +973,56 @@ static int stmmac_init_phy(struct net_de + return 0; + } + +-static void stmmac_display_rings(struct stmmac_priv *priv) ++static void stmmac_display_rx_rings(struct stmmac_priv *priv) + { +- void *head_rx, *head_tx; ++ u32 rx_cnt = priv->plat->rx_queues_to_use; ++ void *head_rx; ++ u32 queue; + +- if (priv->extend_desc) { +- head_rx = (void *)priv->dma_erx; +- head_tx = (void *)priv->dma_etx; +- } else { +- head_rx = (void *)priv->dma_rx; +- head_tx = (void *)priv->dma_tx; ++ /* Display RX rings */ ++ for (queue = 0; queue < rx_cnt; queue++) { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; ++ ++ pr_info("\tRX Queue %u rings\n", queue); ++ ++ if (priv->extend_desc) ++ head_rx = (void *)rx_q->dma_erx; ++ else ++ head_rx = (void *)rx_q->dma_rx; ++ ++ /* Display RX ring */ ++ priv->hw->desc->display_ring(head_rx, DMA_RX_SIZE, true); + } ++} ++ ++static void stmmac_display_tx_rings(struct stmmac_priv *priv) ++{ ++ u32 tx_cnt = priv->plat->tx_queues_to_use; ++ void *head_tx; ++ u32 queue; ++ ++ /* Display TX rings */ ++ for (queue = 0; queue < tx_cnt; queue++) { ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue]; ++ ++ pr_info("\tTX Queue %d rings\n", queue); ++ ++ if (priv->extend_desc) ++ head_tx = (void *)tx_q->dma_etx; ++ else ++ head_tx = (void *)tx_q->dma_tx; + +- /* Display Rx ring */ +- priv->hw->desc->display_ring(head_rx, DMA_RX_SIZE, true); +- /* Display Tx ring */ +- priv->hw->desc->display_ring(head_tx, DMA_TX_SIZE, false); ++ priv->hw->desc->display_ring(head_tx, DMA_TX_SIZE, false); ++ } ++} ++ ++static void stmmac_display_rings(struct stmmac_priv *priv) ++{ ++ /* Display RX ring */ ++ stmmac_display_rx_rings(priv); ++ ++ /* Display TX ring */ ++ stmmac_display_tx_rings(priv); + } + + static int stmmac_set_bfsize(int mtu, int bufsize) +@@ -913,48 +1042,88 @@ static int stmmac_set_bfsize(int mtu, in + } + + /** +- * stmmac_clear_descriptors - clear descriptors ++ * stmmac_clear_rx_descriptors - clear RX descriptors + * @priv: driver private structure +- * Description: this function is called to clear the tx and rx descriptors ++ * @queue: RX queue index ++ * Description: this function is called to clear the RX descriptors + * in case of both basic and extended descriptors are used. + */ +-static void stmmac_clear_descriptors(struct stmmac_priv *priv) ++static void stmmac_clear_rx_descriptors(struct stmmac_priv *priv, u32 queue) + { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; + int i; + +- /* Clear the Rx/Tx descriptors */ ++ /* Clear the RX descriptors */ + for (i = 0; i < DMA_RX_SIZE; i++) + if (priv->extend_desc) +- priv->hw->desc->init_rx_desc(&priv->dma_erx[i].basic, ++ priv->hw->desc->init_rx_desc(&rx_q->dma_erx[i].basic, + priv->use_riwt, priv->mode, + (i == DMA_RX_SIZE - 1)); + else +- priv->hw->desc->init_rx_desc(&priv->dma_rx[i], ++ priv->hw->desc->init_rx_desc(&rx_q->dma_rx[i], + priv->use_riwt, priv->mode, + (i == DMA_RX_SIZE - 1)); ++} ++ ++/** ++ * stmmac_clear_tx_descriptors - clear tx descriptors ++ * @priv: driver private structure ++ * @queue: TX queue index. ++ * Description: this function is called to clear the TX descriptors ++ * in case of both basic and extended descriptors are used. ++ */ ++static void stmmac_clear_tx_descriptors(struct stmmac_priv *priv, u32 queue) ++{ ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue]; ++ int i; ++ ++ /* Clear the TX descriptors */ + for (i = 0; i < DMA_TX_SIZE; i++) + if (priv->extend_desc) +- priv->hw->desc->init_tx_desc(&priv->dma_etx[i].basic, ++ priv->hw->desc->init_tx_desc(&tx_q->dma_etx[i].basic, + priv->mode, + (i == DMA_TX_SIZE - 1)); + else +- priv->hw->desc->init_tx_desc(&priv->dma_tx[i], ++ priv->hw->desc->init_tx_desc(&tx_q->dma_tx[i], + priv->mode, + (i == DMA_TX_SIZE - 1)); + } + + /** ++ * stmmac_clear_descriptors - clear descriptors ++ * @priv: driver private structure ++ * Description: this function is called to clear the TX and RX descriptors ++ * in case of both basic and extended descriptors are used. ++ */ ++static void stmmac_clear_descriptors(struct stmmac_priv *priv) ++{ ++ u32 rx_queue_cnt = priv->plat->rx_queues_to_use; ++ u32 tx_queue_cnt = priv->plat->tx_queues_to_use; ++ u32 queue; ++ ++ /* Clear the RX descriptors */ ++ for (queue = 0; queue < rx_queue_cnt; queue++) ++ stmmac_clear_rx_descriptors(priv, queue); ++ ++ /* Clear the TX descriptors */ ++ for (queue = 0; queue < tx_queue_cnt; queue++) ++ stmmac_clear_tx_descriptors(priv, queue); ++} ++ ++/** + * stmmac_init_rx_buffers - init the RX descriptor buffer. + * @priv: driver private structure + * @p: descriptor pointer + * @i: descriptor index +- * @flags: gfp flag. ++ * @flags: gfp flag ++ * @queue: RX queue index + * Description: this function is called to allocate a receive buffer, perform + * the DMA mapping and init the descriptor. + */ + static int stmmac_init_rx_buffers(struct stmmac_priv *priv, struct dma_desc *p, +- int i, gfp_t flags) ++ int i, gfp_t flags, u32 queue) + { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; + struct sk_buff *skb; + + skb = __netdev_alloc_skb_ip_align(priv->dev, priv->dma_buf_sz, flags); +@@ -963,20 +1132,20 @@ static int stmmac_init_rx_buffers(struct + "%s: Rx init fails; skb is NULL\n", __func__); + return -ENOMEM; + } +- priv->rx_skbuff[i] = skb; +- priv->rx_skbuff_dma[i] = dma_map_single(priv->device, skb->data, ++ rx_q->rx_skbuff[i] = skb; ++ rx_q->rx_skbuff_dma[i] = dma_map_single(priv->device, skb->data, + priv->dma_buf_sz, + DMA_FROM_DEVICE); +- if (dma_mapping_error(priv->device, priv->rx_skbuff_dma[i])) { ++ if (dma_mapping_error(priv->device, rx_q->rx_skbuff_dma[i])) { + netdev_err(priv->dev, "%s: DMA mapping error\n", __func__); + dev_kfree_skb_any(skb); + return -EINVAL; + } + + if (priv->synopsys_id >= DWMAC_CORE_4_00) +- p->des0 = cpu_to_le32(priv->rx_skbuff_dma[i]); ++ p->des0 = cpu_to_le32(rx_q->rx_skbuff_dma[i]); + else +- p->des2 = cpu_to_le32(priv->rx_skbuff_dma[i]); ++ p->des2 = cpu_to_le32(rx_q->rx_skbuff_dma[i]); + + if ((priv->hw->mode->init_desc3) && + (priv->dma_buf_sz == BUF_SIZE_16KiB)) +@@ -985,30 +1154,71 @@ static int stmmac_init_rx_buffers(struct + return 0; + } + +-static void stmmac_free_rx_buffers(struct stmmac_priv *priv, int i) ++/** ++ * stmmac_free_rx_buffer - free RX dma buffers ++ * @priv: private structure ++ * @queue: RX queue index ++ * @i: buffer index. ++ */ ++static void stmmac_free_rx_buffer(struct stmmac_priv *priv, u32 queue, int i) + { +- if (priv->rx_skbuff[i]) { +- dma_unmap_single(priv->device, priv->rx_skbuff_dma[i], ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; ++ ++ if (rx_q->rx_skbuff[i]) { ++ dma_unmap_single(priv->device, rx_q->rx_skbuff_dma[i], + priv->dma_buf_sz, DMA_FROM_DEVICE); +- dev_kfree_skb_any(priv->rx_skbuff[i]); ++ dev_kfree_skb_any(rx_q->rx_skbuff[i]); + } +- priv->rx_skbuff[i] = NULL; ++ rx_q->rx_skbuff[i] = NULL; + } + + /** +- * init_dma_desc_rings - init the RX/TX descriptor rings ++ * stmmac_free_tx_buffer - free RX dma buffers ++ * @priv: private structure ++ * @queue: RX queue index ++ * @i: buffer index. ++ */ ++static void stmmac_free_tx_buffer(struct stmmac_priv *priv, u32 queue, int i) ++{ ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue]; ++ ++ if (tx_q->tx_skbuff_dma[i].buf) { ++ if (tx_q->tx_skbuff_dma[i].map_as_page) ++ dma_unmap_page(priv->device, ++ tx_q->tx_skbuff_dma[i].buf, ++ tx_q->tx_skbuff_dma[i].len, ++ DMA_TO_DEVICE); ++ else ++ dma_unmap_single(priv->device, ++ tx_q->tx_skbuff_dma[i].buf, ++ tx_q->tx_skbuff_dma[i].len, ++ DMA_TO_DEVICE); ++ } ++ ++ if (tx_q->tx_skbuff[i]) { ++ dev_kfree_skb_any(tx_q->tx_skbuff[i]); ++ tx_q->tx_skbuff[i] = NULL; ++ tx_q->tx_skbuff_dma[i].buf = 0; ++ tx_q->tx_skbuff_dma[i].map_as_page = false; ++ } ++} ++ ++/** ++ * init_dma_rx_desc_rings - init the RX descriptor rings + * @dev: net device structure + * @flags: gfp flag. +- * Description: this function initializes the DMA RX/TX descriptors ++ * Description: this function initializes the DMA RX descriptors + * and allocates the socket buffers. It supports the chained and ring + * modes. + */ +-static int init_dma_desc_rings(struct net_device *dev, gfp_t flags) ++static int init_dma_rx_desc_rings(struct net_device *dev, gfp_t flags) + { +- int i; + struct stmmac_priv *priv = netdev_priv(dev); ++ u32 rx_count = priv->plat->rx_queues_to_use; + unsigned int bfsize = 0; + int ret = -ENOMEM; ++ int queue; ++ int i; + + if (priv->hw->mode->set_16kib_bfsize) + bfsize = priv->hw->mode->set_16kib_bfsize(dev->mtu); +@@ -1018,235 +1228,409 @@ static int init_dma_desc_rings(struct ne + + priv->dma_buf_sz = bfsize; + +- netif_dbg(priv, probe, priv->dev, +- "(%s) dma_rx_phy=0x%08x dma_tx_phy=0x%08x\n", +- __func__, (u32)priv->dma_rx_phy, (u32)priv->dma_tx_phy); +- + /* RX INITIALIZATION */ + netif_dbg(priv, probe, priv->dev, + "SKB addresses:\nskb\t\tskb data\tdma data\n"); + +- for (i = 0; i < DMA_RX_SIZE; i++) { +- struct dma_desc *p; +- if (priv->extend_desc) +- p = &((priv->dma_erx + i)->basic); +- else +- p = priv->dma_rx + i; ++ for (queue = 0; queue < rx_count; queue++) { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; ++ ++ netif_dbg(priv, probe, priv->dev, ++ "(%s) dma_rx_phy=0x%08x\n", __func__, ++ (u32)rx_q->dma_rx_phy); ++ ++ for (i = 0; i < DMA_RX_SIZE; i++) { ++ struct dma_desc *p; ++ ++ if (priv->extend_desc) ++ p = &((rx_q->dma_erx + i)->basic); ++ else ++ p = rx_q->dma_rx + i; ++ ++ ret = stmmac_init_rx_buffers(priv, p, i, flags, ++ queue); ++ if (ret) ++ goto err_init_rx_buffers; ++ ++ netif_dbg(priv, probe, priv->dev, "[%p]\t[%p]\t[%x]\n", ++ rx_q->rx_skbuff[i], rx_q->rx_skbuff[i]->data, ++ (unsigned int)rx_q->rx_skbuff_dma[i]); ++ } + +- ret = stmmac_init_rx_buffers(priv, p, i, flags); +- if (ret) +- goto err_init_rx_buffers; ++ rx_q->cur_rx = 0; ++ rx_q->dirty_rx = (unsigned int)(i - DMA_RX_SIZE); + +- netif_dbg(priv, probe, priv->dev, "[%p]\t[%p]\t[%x]\n", +- priv->rx_skbuff[i], priv->rx_skbuff[i]->data, +- (unsigned int)priv->rx_skbuff_dma[i]); ++ stmmac_clear_rx_descriptors(priv, queue); ++ ++ /* Setup the chained descriptor addresses */ ++ if (priv->mode == STMMAC_CHAIN_MODE) { ++ if (priv->extend_desc) ++ priv->hw->mode->init(rx_q->dma_erx, ++ rx_q->dma_rx_phy, ++ DMA_RX_SIZE, 1); ++ else ++ priv->hw->mode->init(rx_q->dma_rx, ++ rx_q->dma_rx_phy, ++ DMA_RX_SIZE, 0); ++ } + } +- priv->cur_rx = 0; +- priv->dirty_rx = (unsigned int)(i - DMA_RX_SIZE); ++ + buf_sz = bfsize; + +- /* Setup the chained descriptor addresses */ +- if (priv->mode == STMMAC_CHAIN_MODE) { +- if (priv->extend_desc) { +- priv->hw->mode->init(priv->dma_erx, priv->dma_rx_phy, +- DMA_RX_SIZE, 1); +- priv->hw->mode->init(priv->dma_etx, priv->dma_tx_phy, +- DMA_TX_SIZE, 1); +- } else { +- priv->hw->mode->init(priv->dma_rx, priv->dma_rx_phy, +- DMA_RX_SIZE, 0); +- priv->hw->mode->init(priv->dma_tx, priv->dma_tx_phy, +- DMA_TX_SIZE, 0); +- } ++ return 0; ++ ++err_init_rx_buffers: ++ while (queue >= 0) { ++ while (--i >= 0) ++ stmmac_free_rx_buffer(priv, queue, i); ++ ++ if (queue == 0) ++ break; ++ ++ i = DMA_RX_SIZE; ++ queue--; + } + +- /* TX INITIALIZATION */ +- for (i = 0; i < DMA_TX_SIZE; i++) { +- struct dma_desc *p; +- if (priv->extend_desc) +- p = &((priv->dma_etx + i)->basic); +- else +- p = priv->dma_tx + i; ++ return ret; ++} + +- if (priv->synopsys_id >= DWMAC_CORE_4_00) { +- p->des0 = 0; +- p->des1 = 0; +- p->des2 = 0; +- p->des3 = 0; +- } else { +- p->des2 = 0; ++/** ++ * init_dma_tx_desc_rings - init the TX descriptor rings ++ * @dev: net device structure. ++ * Description: this function initializes the DMA TX descriptors ++ * and allocates the socket buffers. It supports the chained and ring ++ * modes. ++ */ ++static int init_dma_tx_desc_rings(struct net_device *dev) ++{ ++ struct stmmac_priv *priv = netdev_priv(dev); ++ u32 tx_queue_cnt = priv->plat->tx_queues_to_use; ++ u32 queue; ++ int i; ++ ++ for (queue = 0; queue < tx_queue_cnt; queue++) { ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue]; ++ ++ netif_dbg(priv, probe, priv->dev, ++ "(%s) dma_tx_phy=0x%08x\n", __func__, ++ (u32)tx_q->dma_tx_phy); ++ ++ /* Setup the chained descriptor addresses */ ++ if (priv->mode == STMMAC_CHAIN_MODE) { ++ if (priv->extend_desc) ++ priv->hw->mode->init(tx_q->dma_etx, ++ tx_q->dma_tx_phy, ++ DMA_TX_SIZE, 1); ++ else ++ priv->hw->mode->init(tx_q->dma_tx, ++ tx_q->dma_tx_phy, ++ DMA_TX_SIZE, 0); ++ } ++ ++ for (i = 0; i < DMA_TX_SIZE; i++) { ++ struct dma_desc *p; ++ if (priv->extend_desc) ++ p = &((tx_q->dma_etx + i)->basic); ++ else ++ p = tx_q->dma_tx + i; ++ ++ if (priv->synopsys_id >= DWMAC_CORE_4_00) { ++ p->des0 = 0; ++ p->des1 = 0; ++ p->des2 = 0; ++ p->des3 = 0; ++ } else { ++ p->des2 = 0; ++ } ++ ++ tx_q->tx_skbuff_dma[i].buf = 0; ++ tx_q->tx_skbuff_dma[i].map_as_page = false; ++ tx_q->tx_skbuff_dma[i].len = 0; ++ tx_q->tx_skbuff_dma[i].last_segment = false; ++ tx_q->tx_skbuff[i] = NULL; + } + +- priv->tx_skbuff_dma[i].buf = 0; +- priv->tx_skbuff_dma[i].map_as_page = false; +- priv->tx_skbuff_dma[i].len = 0; +- priv->tx_skbuff_dma[i].last_segment = false; +- priv->tx_skbuff[i] = NULL; ++ tx_q->dirty_tx = 0; ++ tx_q->cur_tx = 0; ++ ++ netdev_tx_reset_queue(netdev_get_tx_queue(priv->dev, queue)); + } + +- priv->dirty_tx = 0; +- priv->cur_tx = 0; +- netdev_reset_queue(priv->dev); ++ return 0; ++} ++ ++/** ++ * init_dma_desc_rings - init the RX/TX descriptor rings ++ * @dev: net device structure ++ * @flags: gfp flag. ++ * Description: this function initializes the DMA RX/TX descriptors ++ * and allocates the socket buffers. It supports the chained and ring ++ * modes. ++ */ ++static int init_dma_desc_rings(struct net_device *dev, gfp_t flags) ++{ ++ struct stmmac_priv *priv = netdev_priv(dev); ++ int ret; ++ ++ ret = init_dma_rx_desc_rings(dev, flags); ++ if (ret) ++ return ret; ++ ++ ret = init_dma_tx_desc_rings(dev); + + stmmac_clear_descriptors(priv); + + if (netif_msg_hw(priv)) + stmmac_display_rings(priv); + +- return 0; +-err_init_rx_buffers: +- while (--i >= 0) +- stmmac_free_rx_buffers(priv, i); + return ret; + } + +-static void dma_free_rx_skbufs(struct stmmac_priv *priv) ++/** ++ * dma_free_rx_skbufs - free RX dma buffers ++ * @priv: private structure ++ * @queue: RX queue index ++ */ ++static void dma_free_rx_skbufs(struct stmmac_priv *priv, u32 queue) + { + int i; + + for (i = 0; i < DMA_RX_SIZE; i++) +- stmmac_free_rx_buffers(priv, i); ++ stmmac_free_rx_buffer(priv, queue, i); + } + +-static void dma_free_tx_skbufs(struct stmmac_priv *priv) ++/** ++ * dma_free_tx_skbufs - free TX dma buffers ++ * @priv: private structure ++ * @queue: TX queue index ++ */ ++static void dma_free_tx_skbufs(struct stmmac_priv *priv, u32 queue) + { + int i; + +- for (i = 0; i < DMA_TX_SIZE; i++) { +- if (priv->tx_skbuff_dma[i].buf) { +- if (priv->tx_skbuff_dma[i].map_as_page) +- dma_unmap_page(priv->device, +- priv->tx_skbuff_dma[i].buf, +- priv->tx_skbuff_dma[i].len, +- DMA_TO_DEVICE); +- else +- dma_unmap_single(priv->device, +- priv->tx_skbuff_dma[i].buf, +- priv->tx_skbuff_dma[i].len, +- DMA_TO_DEVICE); +- } ++ for (i = 0; i < DMA_TX_SIZE; i++) ++ stmmac_free_tx_buffer(priv, queue, i); ++} + +- if (priv->tx_skbuff[i]) { +- dev_kfree_skb_any(priv->tx_skbuff[i]); +- priv->tx_skbuff[i] = NULL; +- priv->tx_skbuff_dma[i].buf = 0; +- priv->tx_skbuff_dma[i].map_as_page = false; +- } ++/** ++ * free_dma_rx_desc_resources - free RX dma desc resources ++ * @priv: private structure ++ */ ++static void free_dma_rx_desc_resources(struct stmmac_priv *priv) ++{ ++ u32 rx_count = priv->plat->rx_queues_to_use; ++ u32 queue; ++ ++ /* Free RX queue resources */ ++ for (queue = 0; queue < rx_count; queue++) { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; ++ ++ /* Release the DMA RX socket buffers */ ++ dma_free_rx_skbufs(priv, queue); ++ ++ /* Free DMA regions of consistent memory previously allocated */ ++ if (!priv->extend_desc) ++ dma_free_coherent(priv->device, ++ DMA_RX_SIZE * sizeof(struct dma_desc), ++ rx_q->dma_rx, rx_q->dma_rx_phy); ++ else ++ dma_free_coherent(priv->device, DMA_RX_SIZE * ++ sizeof(struct dma_extended_desc), ++ rx_q->dma_erx, rx_q->dma_rx_phy); ++ ++ kfree(rx_q->rx_skbuff_dma); ++ kfree(rx_q->rx_skbuff); + } + } + + /** +- * alloc_dma_desc_resources - alloc TX/RX resources. ++ * free_dma_tx_desc_resources - free TX dma desc resources ++ * @priv: private structure ++ */ ++static void free_dma_tx_desc_resources(struct stmmac_priv *priv) ++{ ++ u32 tx_count = priv->plat->tx_queues_to_use; ++ u32 queue = 0; ++ ++ /* Free TX queue resources */ ++ for (queue = 0; queue < tx_count; queue++) { ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue]; ++ ++ /* Release the DMA TX socket buffers */ ++ dma_free_tx_skbufs(priv, queue); ++ ++ /* Free DMA regions of consistent memory previously allocated */ ++ if (!priv->extend_desc) ++ dma_free_coherent(priv->device, ++ DMA_TX_SIZE * sizeof(struct dma_desc), ++ tx_q->dma_tx, tx_q->dma_tx_phy); ++ else ++ dma_free_coherent(priv->device, DMA_TX_SIZE * ++ sizeof(struct dma_extended_desc), ++ tx_q->dma_etx, tx_q->dma_tx_phy); ++ ++ kfree(tx_q->tx_skbuff_dma); ++ kfree(tx_q->tx_skbuff); ++ } ++} ++ ++/** ++ * alloc_dma_rx_desc_resources - alloc RX resources. + * @priv: private structure + * Description: according to which descriptor can be used (extend or basic) + * this function allocates the resources for TX and RX paths. In case of + * reception, for example, it pre-allocated the RX socket buffer in order to + * allow zero-copy mechanism. + */ +-static int alloc_dma_desc_resources(struct stmmac_priv *priv) ++static int alloc_dma_rx_desc_resources(struct stmmac_priv *priv) + { ++ u32 rx_count = priv->plat->rx_queues_to_use; + int ret = -ENOMEM; ++ u32 queue; + +- priv->rx_skbuff_dma = kmalloc_array(DMA_RX_SIZE, sizeof(dma_addr_t), +- GFP_KERNEL); +- if (!priv->rx_skbuff_dma) +- return -ENOMEM; ++ /* RX queues buffers and DMA */ ++ for (queue = 0; queue < rx_count; queue++) { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; + +- priv->rx_skbuff = kmalloc_array(DMA_RX_SIZE, sizeof(struct sk_buff *), +- GFP_KERNEL); +- if (!priv->rx_skbuff) +- goto err_rx_skbuff; +- +- priv->tx_skbuff_dma = kmalloc_array(DMA_TX_SIZE, +- sizeof(*priv->tx_skbuff_dma), +- GFP_KERNEL); +- if (!priv->tx_skbuff_dma) +- goto err_tx_skbuff_dma; +- +- priv->tx_skbuff = kmalloc_array(DMA_TX_SIZE, sizeof(struct sk_buff *), +- GFP_KERNEL); +- if (!priv->tx_skbuff) +- goto err_tx_skbuff; +- +- if (priv->extend_desc) { +- priv->dma_erx = dma_zalloc_coherent(priv->device, DMA_RX_SIZE * +- sizeof(struct +- dma_extended_desc), +- &priv->dma_rx_phy, +- GFP_KERNEL); +- if (!priv->dma_erx) +- goto err_dma; ++ rx_q->queue_index = queue; ++ rx_q->priv_data = priv; + +- priv->dma_etx = dma_zalloc_coherent(priv->device, DMA_TX_SIZE * +- sizeof(struct +- dma_extended_desc), +- &priv->dma_tx_phy, ++ rx_q->rx_skbuff_dma = kmalloc_array(DMA_RX_SIZE, ++ sizeof(dma_addr_t), + GFP_KERNEL); +- if (!priv->dma_etx) { +- dma_free_coherent(priv->device, DMA_RX_SIZE * +- sizeof(struct dma_extended_desc), +- priv->dma_erx, priv->dma_rx_phy); +- goto err_dma; +- } +- } else { +- priv->dma_rx = dma_zalloc_coherent(priv->device, DMA_RX_SIZE * +- sizeof(struct dma_desc), +- &priv->dma_rx_phy, +- GFP_KERNEL); +- if (!priv->dma_rx) +- goto err_dma; ++ if (!rx_q->rx_skbuff_dma) ++ return -ENOMEM; + +- priv->dma_tx = dma_zalloc_coherent(priv->device, DMA_TX_SIZE * +- sizeof(struct dma_desc), +- &priv->dma_tx_phy, +- GFP_KERNEL); +- if (!priv->dma_tx) { +- dma_free_coherent(priv->device, DMA_RX_SIZE * +- sizeof(struct dma_desc), +- priv->dma_rx, priv->dma_rx_phy); ++ rx_q->rx_skbuff = kmalloc_array(DMA_RX_SIZE, ++ sizeof(struct sk_buff *), ++ GFP_KERNEL); ++ if (!rx_q->rx_skbuff) + goto err_dma; ++ ++ if (priv->extend_desc) { ++ rx_q->dma_erx = dma_zalloc_coherent(priv->device, ++ DMA_RX_SIZE * ++ sizeof(struct ++ dma_extended_desc), ++ &rx_q->dma_rx_phy, ++ GFP_KERNEL); ++ if (!rx_q->dma_erx) ++ goto err_dma; ++ ++ } else { ++ rx_q->dma_rx = dma_zalloc_coherent(priv->device, ++ DMA_RX_SIZE * ++ sizeof(struct ++ dma_desc), ++ &rx_q->dma_rx_phy, ++ GFP_KERNEL); ++ if (!rx_q->dma_rx) ++ goto err_dma; + } + } + + return 0; + + err_dma: +- kfree(priv->tx_skbuff); +-err_tx_skbuff: +- kfree(priv->tx_skbuff_dma); +-err_tx_skbuff_dma: +- kfree(priv->rx_skbuff); +-err_rx_skbuff: +- kfree(priv->rx_skbuff_dma); ++ free_dma_rx_desc_resources(priv); ++ ++ return ret; ++} ++ ++/** ++ * alloc_dma_tx_desc_resources - alloc TX resources. ++ * @priv: private structure ++ * Description: according to which descriptor can be used (extend or basic) ++ * this function allocates the resources for TX and RX paths. In case of ++ * reception, for example, it pre-allocated the RX socket buffer in order to ++ * allow zero-copy mechanism. ++ */ ++static int alloc_dma_tx_desc_resources(struct stmmac_priv *priv) ++{ ++ u32 tx_count = priv->plat->tx_queues_to_use; ++ int ret = -ENOMEM; ++ u32 queue; ++ ++ /* TX queues buffers and DMA */ ++ for (queue = 0; queue < tx_count; queue++) { ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue]; ++ ++ tx_q->queue_index = queue; ++ tx_q->priv_data = priv; ++ ++ tx_q->tx_skbuff_dma = kmalloc_array(DMA_TX_SIZE, ++ sizeof(*tx_q->tx_skbuff_dma), ++ GFP_KERNEL); ++ if (!tx_q->tx_skbuff_dma) ++ return -ENOMEM; ++ ++ tx_q->tx_skbuff = kmalloc_array(DMA_TX_SIZE, ++ sizeof(struct sk_buff *), ++ GFP_KERNEL); ++ if (!tx_q->tx_skbuff) ++ goto err_dma_buffers; ++ ++ if (priv->extend_desc) { ++ tx_q->dma_etx = dma_zalloc_coherent(priv->device, ++ DMA_TX_SIZE * ++ sizeof(struct ++ dma_extended_desc), ++ &tx_q->dma_tx_phy, ++ GFP_KERNEL); ++ if (!tx_q->dma_etx) ++ goto err_dma_buffers; ++ } else { ++ tx_q->dma_tx = dma_zalloc_coherent(priv->device, ++ DMA_TX_SIZE * ++ sizeof(struct ++ dma_desc), ++ &tx_q->dma_tx_phy, ++ GFP_KERNEL); ++ if (!tx_q->dma_tx) ++ goto err_dma_buffers; ++ } ++ } ++ ++ return 0; ++ ++err_dma_buffers: ++ free_dma_tx_desc_resources(priv); ++ + return ret; + } + ++/** ++ * alloc_dma_desc_resources - alloc TX/RX resources. ++ * @priv: private structure ++ * Description: according to which descriptor can be used (extend or basic) ++ * this function allocates the resources for TX and RX paths. In case of ++ * reception, for example, it pre-allocated the RX socket buffer in order to ++ * allow zero-copy mechanism. ++ */ ++static int alloc_dma_desc_resources(struct stmmac_priv *priv) ++{ ++ /* RX Allocation */ ++ int ret = alloc_dma_rx_desc_resources(priv); ++ ++ if (ret) ++ return ret; ++ ++ ret = alloc_dma_tx_desc_resources(priv); ++ ++ return ret; ++} ++ ++/** ++ * free_dma_desc_resources - free dma desc resources ++ * @priv: private structure ++ */ + static void free_dma_desc_resources(struct stmmac_priv *priv) + { +- /* Release the DMA TX/RX socket buffers */ +- dma_free_rx_skbufs(priv); +- dma_free_tx_skbufs(priv); +- +- /* Free DMA regions of consistent memory previously allocated */ +- if (!priv->extend_desc) { +- dma_free_coherent(priv->device, +- DMA_TX_SIZE * sizeof(struct dma_desc), +- priv->dma_tx, priv->dma_tx_phy); +- dma_free_coherent(priv->device, +- DMA_RX_SIZE * sizeof(struct dma_desc), +- priv->dma_rx, priv->dma_rx_phy); +- } else { +- dma_free_coherent(priv->device, DMA_TX_SIZE * +- sizeof(struct dma_extended_desc), +- priv->dma_etx, priv->dma_tx_phy); +- dma_free_coherent(priv->device, DMA_RX_SIZE * +- sizeof(struct dma_extended_desc), +- priv->dma_erx, priv->dma_rx_phy); +- } +- kfree(priv->rx_skbuff_dma); +- kfree(priv->rx_skbuff); +- kfree(priv->tx_skbuff_dma); +- kfree(priv->tx_skbuff); ++ /* Release the DMA RX socket buffers */ ++ free_dma_rx_desc_resources(priv); ++ ++ /* Release the DMA TX socket buffers */ ++ free_dma_tx_desc_resources(priv); + } + + /** +@@ -1256,19 +1640,104 @@ static void free_dma_desc_resources(stru + */ + static void stmmac_mac_enable_rx_queues(struct stmmac_priv *priv) + { +- int rx_count = priv->dma_cap.number_rx_queues; +- int queue = 0; ++ u32 rx_queues_count = priv->plat->rx_queues_to_use; ++ int queue; ++ u8 mode; + +- /* If GMAC does not have multiple queues, then this is not necessary*/ +- if (rx_count == 1) +- return; ++ for (queue = 0; queue < rx_queues_count; queue++) { ++ mode = priv->plat->rx_queues_cfg[queue].mode_to_use; ++ priv->hw->mac->rx_queue_enable(priv->hw, mode, queue); ++ } ++} + +- /** +- * If the core is synthesized with multiple rx queues / multiple +- * dma channels, then rx queues will be disabled by default. +- * For now only rx queue 0 is enabled. +- */ +- priv->hw->mac->rx_queue_enable(priv->hw, queue); ++/** ++ * stmmac_start_rx_dma - start RX DMA channel ++ * @priv: driver private structure ++ * @chan: RX channel index ++ * Description: ++ * This starts a RX DMA channel ++ */ ++static void stmmac_start_rx_dma(struct stmmac_priv *priv, u32 chan) ++{ ++ netdev_dbg(priv->dev, "DMA RX processes started in channel %d\n", chan); ++ priv->hw->dma->start_rx(priv->ioaddr, chan); ++} ++ ++/** ++ * stmmac_start_tx_dma - start TX DMA channel ++ * @priv: driver private structure ++ * @chan: TX channel index ++ * Description: ++ * This starts a TX DMA channel ++ */ ++static void stmmac_start_tx_dma(struct stmmac_priv *priv, u32 chan) ++{ ++ netdev_dbg(priv->dev, "DMA TX processes started in channel %d\n", chan); ++ priv->hw->dma->start_tx(priv->ioaddr, chan); ++} ++ ++/** ++ * stmmac_stop_rx_dma - stop RX DMA channel ++ * @priv: driver private structure ++ * @chan: RX channel index ++ * Description: ++ * This stops a RX DMA channel ++ */ ++static void stmmac_stop_rx_dma(struct stmmac_priv *priv, u32 chan) ++{ ++ netdev_dbg(priv->dev, "DMA RX processes stopped in channel %d\n", chan); ++ priv->hw->dma->stop_rx(priv->ioaddr, chan); ++} ++ ++/** ++ * stmmac_stop_tx_dma - stop TX DMA channel ++ * @priv: driver private structure ++ * @chan: TX channel index ++ * Description: ++ * This stops a TX DMA channel ++ */ ++static void stmmac_stop_tx_dma(struct stmmac_priv *priv, u32 chan) ++{ ++ netdev_dbg(priv->dev, "DMA TX processes stopped in channel %d\n", chan); ++ priv->hw->dma->stop_tx(priv->ioaddr, chan); ++} ++ ++/** ++ * stmmac_start_all_dma - start all RX and TX DMA channels ++ * @priv: driver private structure ++ * Description: ++ * This starts all the RX and TX DMA channels ++ */ ++static void stmmac_start_all_dma(struct stmmac_priv *priv) ++{ ++ u32 rx_channels_count = priv->plat->rx_queues_to_use; ++ u32 tx_channels_count = priv->plat->tx_queues_to_use; ++ u32 chan = 0; ++ ++ for (chan = 0; chan < rx_channels_count; chan++) ++ stmmac_start_rx_dma(priv, chan); ++ ++ for (chan = 0; chan < tx_channels_count; chan++) ++ stmmac_start_tx_dma(priv, chan); ++} ++ ++/** ++ * stmmac_stop_all_dma - stop all RX and TX DMA channels ++ * @priv: driver private structure ++ * Description: ++ * This stops the RX and TX DMA channels ++ */ ++static void stmmac_stop_all_dma(struct stmmac_priv *priv) ++{ ++ u32 rx_channels_count = priv->plat->rx_queues_to_use; ++ u32 tx_channels_count = priv->plat->tx_queues_to_use; ++ u32 chan = 0; ++ ++ for (chan = 0; chan < rx_channels_count; chan++) ++ stmmac_stop_rx_dma(priv, chan); ++ ++ for (chan = 0; chan < tx_channels_count; chan++) ++ stmmac_stop_tx_dma(priv, chan); + } + + /** +@@ -1279,11 +1748,20 @@ static void stmmac_mac_enable_rx_queues( + */ + static void stmmac_dma_operation_mode(struct stmmac_priv *priv) + { ++ u32 rx_channels_count = priv->plat->rx_queues_to_use; ++ u32 tx_channels_count = priv->plat->tx_queues_to_use; + int rxfifosz = priv->plat->rx_fifo_size; +- +- if (priv->plat->force_thresh_dma_mode) +- priv->hw->dma->dma_mode(priv->ioaddr, tc, tc, rxfifosz); +- else if (priv->plat->force_sf_dma_mode || priv->plat->tx_coe) { ++ u32 txmode = 0; ++ u32 rxmode = 0; ++ u32 chan = 0; ++ ++ if (rxfifosz == 0) ++ rxfifosz = priv->dma_cap.rx_fifo_size; ++ ++ if (priv->plat->force_thresh_dma_mode) { ++ txmode = tc; ++ rxmode = tc; ++ } else if (priv->plat->force_sf_dma_mode || priv->plat->tx_coe) { + /* + * In case of GMAC, SF mode can be enabled + * to perform the TX COE in HW. This depends on: +@@ -1291,37 +1769,53 @@ static void stmmac_dma_operation_mode(st + * 2) There is no bugged Jumbo frame support + * that needs to not insert csum in the TDES. + */ +- priv->hw->dma->dma_mode(priv->ioaddr, SF_DMA_MODE, SF_DMA_MODE, +- rxfifosz); ++ txmode = SF_DMA_MODE; ++ rxmode = SF_DMA_MODE; + priv->xstats.threshold = SF_DMA_MODE; +- } else +- priv->hw->dma->dma_mode(priv->ioaddr, tc, SF_DMA_MODE, ++ } else { ++ txmode = tc; ++ rxmode = SF_DMA_MODE; ++ } ++ ++ /* configure all channels */ ++ if (priv->synopsys_id >= DWMAC_CORE_4_00) { ++ for (chan = 0; chan < rx_channels_count; chan++) ++ priv->hw->dma->dma_rx_mode(priv->ioaddr, rxmode, chan, ++ rxfifosz); ++ ++ for (chan = 0; chan < tx_channels_count; chan++) ++ priv->hw->dma->dma_tx_mode(priv->ioaddr, txmode, chan); ++ } else { ++ priv->hw->dma->dma_mode(priv->ioaddr, txmode, rxmode, + rxfifosz); ++ } + } + + /** + * stmmac_tx_clean - to manage the transmission completion + * @priv: driver private structure ++ * @queue: TX queue index + * Description: it reclaims the transmit resources after transmission completes. + */ +-static void stmmac_tx_clean(struct stmmac_priv *priv) ++static void stmmac_tx_clean(struct stmmac_priv *priv, u32 queue) + { ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue]; + unsigned int bytes_compl = 0, pkts_compl = 0; +- unsigned int entry = priv->dirty_tx; ++ unsigned int entry = tx_q->dirty_tx; + + netif_tx_lock(priv->dev); + + priv->xstats.tx_clean++; + +- while (entry != priv->cur_tx) { +- struct sk_buff *skb = priv->tx_skbuff[entry]; ++ while (entry != tx_q->cur_tx) { ++ struct sk_buff *skb = tx_q->tx_skbuff[entry]; + struct dma_desc *p; + int status; + + if (priv->extend_desc) +- p = (struct dma_desc *)(priv->dma_etx + entry); ++ p = (struct dma_desc *)(tx_q->dma_etx + entry); + else +- p = priv->dma_tx + entry; ++ p = tx_q->dma_tx + entry; + + status = priv->hw->desc->tx_status(&priv->dev->stats, + &priv->xstats, p, +@@ -1342,48 +1836,51 @@ static void stmmac_tx_clean(struct stmma + stmmac_get_tx_hwtstamp(priv, p, skb); + } + +- if (likely(priv->tx_skbuff_dma[entry].buf)) { +- if (priv->tx_skbuff_dma[entry].map_as_page) ++ if (likely(tx_q->tx_skbuff_dma[entry].buf)) { ++ if (tx_q->tx_skbuff_dma[entry].map_as_page) + dma_unmap_page(priv->device, +- priv->tx_skbuff_dma[entry].buf, +- priv->tx_skbuff_dma[entry].len, ++ tx_q->tx_skbuff_dma[entry].buf, ++ tx_q->tx_skbuff_dma[entry].len, + DMA_TO_DEVICE); + else + dma_unmap_single(priv->device, +- priv->tx_skbuff_dma[entry].buf, +- priv->tx_skbuff_dma[entry].len, ++ tx_q->tx_skbuff_dma[entry].buf, ++ tx_q->tx_skbuff_dma[entry].len, + DMA_TO_DEVICE); +- priv->tx_skbuff_dma[entry].buf = 0; +- priv->tx_skbuff_dma[entry].len = 0; +- priv->tx_skbuff_dma[entry].map_as_page = false; ++ tx_q->tx_skbuff_dma[entry].buf = 0; ++ tx_q->tx_skbuff_dma[entry].len = 0; ++ tx_q->tx_skbuff_dma[entry].map_as_page = false; + } + + if (priv->hw->mode->clean_desc3) +- priv->hw->mode->clean_desc3(priv, p); ++ priv->hw->mode->clean_desc3(tx_q, p); + +- priv->tx_skbuff_dma[entry].last_segment = false; +- priv->tx_skbuff_dma[entry].is_jumbo = false; ++ tx_q->tx_skbuff_dma[entry].last_segment = false; ++ tx_q->tx_skbuff_dma[entry].is_jumbo = false; + + if (likely(skb != NULL)) { + pkts_compl++; + bytes_compl += skb->len; + dev_consume_skb_any(skb); +- priv->tx_skbuff[entry] = NULL; ++ tx_q->tx_skbuff[entry] = NULL; + } + + priv->hw->desc->release_tx_desc(p, priv->mode); + + entry = STMMAC_GET_ENTRY(entry, DMA_TX_SIZE); + } +- priv->dirty_tx = entry; ++ tx_q->dirty_tx = entry; ++ ++ netdev_tx_completed_queue(netdev_get_tx_queue(priv->dev, queue), ++ pkts_compl, bytes_compl); + +- netdev_completed_queue(priv->dev, pkts_compl, bytes_compl); ++ if (unlikely(netif_tx_queue_stopped(netdev_get_tx_queue(priv->dev, ++ queue))) && ++ stmmac_tx_avail(priv, queue) > STMMAC_TX_THRESH) { + +- if (unlikely(netif_queue_stopped(priv->dev) && +- stmmac_tx_avail(priv) > STMMAC_TX_THRESH)) { + netif_dbg(priv, tx_done, priv->dev, + "%s: restart transmit\n", __func__); +- netif_wake_queue(priv->dev); ++ netif_tx_wake_queue(netdev_get_tx_queue(priv->dev, queue)); + } + + if ((priv->eee_enabled) && (!priv->tx_path_in_lpi_mode)) { +@@ -1393,45 +1890,76 @@ static void stmmac_tx_clean(struct stmma + netif_tx_unlock(priv->dev); + } + +-static inline void stmmac_enable_dma_irq(struct stmmac_priv *priv) ++static inline void stmmac_enable_dma_irq(struct stmmac_priv *priv, u32 chan) + { +- priv->hw->dma->enable_dma_irq(priv->ioaddr); ++ priv->hw->dma->enable_dma_irq(priv->ioaddr, chan); + } + +-static inline void stmmac_disable_dma_irq(struct stmmac_priv *priv) ++static inline void stmmac_disable_dma_irq(struct stmmac_priv *priv, u32 chan) + { +- priv->hw->dma->disable_dma_irq(priv->ioaddr); ++ priv->hw->dma->disable_dma_irq(priv->ioaddr, chan); + } + + /** + * stmmac_tx_err - to manage the tx error + * @priv: driver private structure ++ * @chan: channel index + * Description: it cleans the descriptors and restarts the transmission + * in case of transmission errors. + */ +-static void stmmac_tx_err(struct stmmac_priv *priv) ++static void stmmac_tx_err(struct stmmac_priv *priv, u32 chan) + { ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[chan]; + int i; +- netif_stop_queue(priv->dev); + +- priv->hw->dma->stop_tx(priv->ioaddr); +- dma_free_tx_skbufs(priv); ++ netif_tx_stop_queue(netdev_get_tx_queue(priv->dev, chan)); ++ ++ stmmac_stop_tx_dma(priv, chan); ++ dma_free_tx_skbufs(priv, chan); + for (i = 0; i < DMA_TX_SIZE; i++) + if (priv->extend_desc) +- priv->hw->desc->init_tx_desc(&priv->dma_etx[i].basic, ++ priv->hw->desc->init_tx_desc(&tx_q->dma_etx[i].basic, + priv->mode, + (i == DMA_TX_SIZE - 1)); + else +- priv->hw->desc->init_tx_desc(&priv->dma_tx[i], ++ priv->hw->desc->init_tx_desc(&tx_q->dma_tx[i], + priv->mode, + (i == DMA_TX_SIZE - 1)); +- priv->dirty_tx = 0; +- priv->cur_tx = 0; +- netdev_reset_queue(priv->dev); +- priv->hw->dma->start_tx(priv->ioaddr); ++ tx_q->dirty_tx = 0; ++ tx_q->cur_tx = 0; ++ netdev_tx_reset_queue(netdev_get_tx_queue(priv->dev, chan)); ++ stmmac_start_tx_dma(priv, chan); + + priv->dev->stats.tx_errors++; +- netif_wake_queue(priv->dev); ++ netif_tx_wake_queue(netdev_get_tx_queue(priv->dev, chan)); ++} ++ ++/** ++ * stmmac_set_dma_operation_mode - Set DMA operation mode by channel ++ * @priv: driver private structure ++ * @txmode: TX operating mode ++ * @rxmode: RX operating mode ++ * @chan: channel index ++ * Description: it is used for configuring of the DMA operation mode in ++ * runtime in order to program the tx/rx DMA thresholds or Store-And-Forward ++ * mode. ++ */ ++static void stmmac_set_dma_operation_mode(struct stmmac_priv *priv, u32 txmode, ++ u32 rxmode, u32 chan) ++{ ++ int rxfifosz = priv->plat->rx_fifo_size; ++ ++ if (rxfifosz == 0) ++ rxfifosz = priv->dma_cap.rx_fifo_size; ++ ++ if (priv->synopsys_id >= DWMAC_CORE_4_00) { ++ priv->hw->dma->dma_rx_mode(priv->ioaddr, rxmode, chan, ++ rxfifosz); ++ priv->hw->dma->dma_tx_mode(priv->ioaddr, txmode, chan); ++ } else { ++ priv->hw->dma->dma_mode(priv->ioaddr, txmode, rxmode, ++ rxfifosz); ++ } + } + + /** +@@ -1443,31 +1971,43 @@ static void stmmac_tx_err(struct stmmac_ + */ + static void stmmac_dma_interrupt(struct stmmac_priv *priv) + { ++ u32 tx_channel_count = priv->plat->tx_queues_to_use; + int status; +- int rxfifosz = priv->plat->rx_fifo_size; ++ u32 chan; ++ ++ for (chan = 0; chan < tx_channel_count; chan++) { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[chan]; + +- status = priv->hw->dma->dma_interrupt(priv->ioaddr, &priv->xstats); +- if (likely((status & handle_rx)) || (status & handle_tx)) { +- if (likely(napi_schedule_prep(&priv->napi))) { +- stmmac_disable_dma_irq(priv); +- __napi_schedule(&priv->napi); ++ status = priv->hw->dma->dma_interrupt(priv->ioaddr, ++ &priv->xstats, chan); ++ if (likely((status & handle_rx)) || (status & handle_tx)) { ++ if (likely(napi_schedule_prep(&rx_q->napi))) { ++ stmmac_disable_dma_irq(priv, chan); ++ __napi_schedule(&rx_q->napi); ++ } + } +- } +- if (unlikely(status & tx_hard_error_bump_tc)) { +- /* Try to bump up the dma threshold on this failure */ +- if (unlikely(priv->xstats.threshold != SF_DMA_MODE) && +- (tc <= 256)) { +- tc += 64; +- if (priv->plat->force_thresh_dma_mode) +- priv->hw->dma->dma_mode(priv->ioaddr, tc, tc, +- rxfifosz); +- else +- priv->hw->dma->dma_mode(priv->ioaddr, tc, +- SF_DMA_MODE, rxfifosz); +- priv->xstats.threshold = tc; ++ ++ if (unlikely(status & tx_hard_error_bump_tc)) { ++ /* Try to bump up the dma threshold on this failure */ ++ if (unlikely(priv->xstats.threshold != SF_DMA_MODE) && ++ (tc <= 256)) { ++ tc += 64; ++ if (priv->plat->force_thresh_dma_mode) ++ stmmac_set_dma_operation_mode(priv, ++ tc, ++ tc, ++ chan); ++ else ++ stmmac_set_dma_operation_mode(priv, ++ tc, ++ SF_DMA_MODE, ++ chan); ++ priv->xstats.threshold = tc; ++ } ++ } else if (unlikely(status == tx_hard_error)) { ++ stmmac_tx_err(priv, chan); + } +- } else if (unlikely(status == tx_hard_error)) +- stmmac_tx_err(priv); ++ } + } + + /** +@@ -1574,6 +2114,13 @@ static void stmmac_check_ether_addr(stru + */ + static int stmmac_init_dma_engine(struct stmmac_priv *priv) + { ++ u32 rx_channels_count = priv->plat->rx_queues_to_use; ++ u32 tx_channels_count = priv->plat->tx_queues_to_use; ++ struct stmmac_rx_queue *rx_q; ++ struct stmmac_tx_queue *tx_q; ++ u32 dummy_dma_rx_phy = 0; ++ u32 dummy_dma_tx_phy = 0; ++ u32 chan = 0; + int atds = 0; + int ret = 0; + +@@ -1591,19 +2138,49 @@ static int stmmac_init_dma_engine(struct + return ret; + } + +- priv->hw->dma->init(priv->ioaddr, priv->plat->dma_cfg, +- priv->dma_tx_phy, priv->dma_rx_phy, atds); +- + if (priv->synopsys_id >= DWMAC_CORE_4_00) { +- priv->rx_tail_addr = priv->dma_rx_phy + +- (DMA_RX_SIZE * sizeof(struct dma_desc)); +- priv->hw->dma->set_rx_tail_ptr(priv->ioaddr, priv->rx_tail_addr, +- STMMAC_CHAN0); +- +- priv->tx_tail_addr = priv->dma_tx_phy + +- (DMA_TX_SIZE * sizeof(struct dma_desc)); +- priv->hw->dma->set_tx_tail_ptr(priv->ioaddr, priv->tx_tail_addr, +- STMMAC_CHAN0); ++ /* DMA Configuration */ ++ priv->hw->dma->init(priv->ioaddr, priv->plat->dma_cfg, ++ dummy_dma_tx_phy, dummy_dma_rx_phy, atds); ++ ++ /* DMA RX Channel Configuration */ ++ for (chan = 0; chan < rx_channels_count; chan++) { ++ rx_q = &priv->rx_queue[chan]; ++ ++ priv->hw->dma->init_rx_chan(priv->ioaddr, ++ priv->plat->dma_cfg, ++ rx_q->dma_rx_phy, chan); ++ ++ rx_q->rx_tail_addr = rx_q->dma_rx_phy + ++ (DMA_RX_SIZE * sizeof(struct dma_desc)); ++ priv->hw->dma->set_rx_tail_ptr(priv->ioaddr, ++ rx_q->rx_tail_addr, ++ chan); ++ } ++ ++ /* DMA TX Channel Configuration */ ++ for (chan = 0; chan < tx_channels_count; chan++) { ++ tx_q = &priv->tx_queue[chan]; ++ ++ priv->hw->dma->init_chan(priv->ioaddr, ++ priv->plat->dma_cfg, ++ chan); ++ ++ priv->hw->dma->init_tx_chan(priv->ioaddr, ++ priv->plat->dma_cfg, ++ tx_q->dma_tx_phy, chan); ++ ++ tx_q->tx_tail_addr = tx_q->dma_tx_phy + ++ (DMA_TX_SIZE * sizeof(struct dma_desc)); ++ priv->hw->dma->set_tx_tail_ptr(priv->ioaddr, ++ tx_q->tx_tail_addr, ++ chan); ++ } ++ } else { ++ rx_q = &priv->rx_queue[chan]; ++ tx_q = &priv->tx_queue[chan]; ++ priv->hw->dma->init(priv->ioaddr, priv->plat->dma_cfg, ++ tx_q->dma_tx_phy, rx_q->dma_rx_phy, atds); + } + + if (priv->plat->axi && priv->hw->dma->axi) +@@ -1621,8 +2198,12 @@ static int stmmac_init_dma_engine(struct + static void stmmac_tx_timer(unsigned long data) + { + struct stmmac_priv *priv = (struct stmmac_priv *)data; ++ u32 tx_queues_count = priv->plat->tx_queues_to_use; ++ u32 queue; + +- stmmac_tx_clean(priv); ++ /* let's scan all the tx queues */ ++ for (queue = 0; queue < tx_queues_count; queue++) ++ stmmac_tx_clean(priv, queue); + } + + /** +@@ -1644,6 +2225,196 @@ static void stmmac_init_tx_coalesce(stru + add_timer(&priv->txtimer); + } + ++static void stmmac_set_rings_length(struct stmmac_priv *priv) ++{ ++ u32 rx_channels_count = priv->plat->rx_queues_to_use; ++ u32 tx_channels_count = priv->plat->tx_queues_to_use; ++ u32 chan; ++ ++ /* set TX ring length */ ++ if (priv->hw->dma->set_tx_ring_len) { ++ for (chan = 0; chan < tx_channels_count; chan++) ++ priv->hw->dma->set_tx_ring_len(priv->ioaddr, ++ (DMA_TX_SIZE - 1), chan); ++ } ++ ++ /* set RX ring length */ ++ if (priv->hw->dma->set_rx_ring_len) { ++ for (chan = 0; chan < rx_channels_count; chan++) ++ priv->hw->dma->set_rx_ring_len(priv->ioaddr, ++ (DMA_RX_SIZE - 1), chan); ++ } ++} ++ ++/** ++ * stmmac_set_tx_queue_weight - Set TX queue weight ++ * @priv: driver private structure ++ * Description: It is used for setting TX queues weight ++ */ ++static void stmmac_set_tx_queue_weight(struct stmmac_priv *priv) ++{ ++ u32 tx_queues_count = priv->plat->tx_queues_to_use; ++ u32 weight; ++ u32 queue; ++ ++ for (queue = 0; queue < tx_queues_count; queue++) { ++ weight = priv->plat->tx_queues_cfg[queue].weight; ++ priv->hw->mac->set_mtl_tx_queue_weight(priv->hw, weight, queue); ++ } ++} ++ ++/** ++ * stmmac_configure_cbs - Configure CBS in TX queue ++ * @priv: driver private structure ++ * Description: It is used for configuring CBS in AVB TX queues ++ */ ++static void stmmac_configure_cbs(struct stmmac_priv *priv) ++{ ++ u32 tx_queues_count = priv->plat->tx_queues_to_use; ++ u32 mode_to_use; ++ u32 queue; ++ ++ /* queue 0 is reserved for legacy traffic */ ++ for (queue = 1; queue < tx_queues_count; queue++) { ++ mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use; ++ if (mode_to_use == MTL_QUEUE_DCB) ++ continue; ++ ++ priv->hw->mac->config_cbs(priv->hw, ++ priv->plat->tx_queues_cfg[queue].send_slope, ++ priv->plat->tx_queues_cfg[queue].idle_slope, ++ priv->plat->tx_queues_cfg[queue].high_credit, ++ priv->plat->tx_queues_cfg[queue].low_credit, ++ queue); ++ } ++} ++ ++/** ++ * stmmac_rx_queue_dma_chan_map - Map RX queue to RX dma channel ++ * @priv: driver private structure ++ * Description: It is used for mapping RX queues to RX dma channels ++ */ ++static void stmmac_rx_queue_dma_chan_map(struct stmmac_priv *priv) ++{ ++ u32 rx_queues_count = priv->plat->rx_queues_to_use; ++ u32 queue; ++ u32 chan; ++ ++ for (queue = 0; queue < rx_queues_count; queue++) { ++ chan = priv->plat->rx_queues_cfg[queue].chan; ++ priv->hw->mac->map_mtl_to_dma(priv->hw, queue, chan); ++ } ++} ++ ++/** ++ * stmmac_mac_config_rx_queues_prio - Configure RX Queue priority ++ * @priv: driver private structure ++ * Description: It is used for configuring the RX Queue Priority ++ */ ++static void stmmac_mac_config_rx_queues_prio(struct stmmac_priv *priv) ++{ ++ u32 rx_queues_count = priv->plat->rx_queues_to_use; ++ u32 queue; ++ u32 prio; ++ ++ for (queue = 0; queue < rx_queues_count; queue++) { ++ if (!priv->plat->rx_queues_cfg[queue].use_prio) ++ continue; ++ ++ prio = priv->plat->rx_queues_cfg[queue].prio; ++ priv->hw->mac->rx_queue_prio(priv->hw, prio, queue); ++ } ++} ++ ++/** ++ * stmmac_mac_config_tx_queues_prio - Configure TX Queue priority ++ * @priv: driver private structure ++ * Description: It is used for configuring the TX Queue Priority ++ */ ++static void stmmac_mac_config_tx_queues_prio(struct stmmac_priv *priv) ++{ ++ u32 tx_queues_count = priv->plat->tx_queues_to_use; ++ u32 queue; ++ u32 prio; ++ ++ for (queue = 0; queue < tx_queues_count; queue++) { ++ if (!priv->plat->tx_queues_cfg[queue].use_prio) ++ continue; ++ ++ prio = priv->plat->tx_queues_cfg[queue].prio; ++ priv->hw->mac->tx_queue_prio(priv->hw, prio, queue); ++ } ++} ++ ++/** ++ * stmmac_mac_config_rx_queues_routing - Configure RX Queue Routing ++ * @priv: driver private structure ++ * Description: It is used for configuring the RX queue routing ++ */ ++static void stmmac_mac_config_rx_queues_routing(struct stmmac_priv *priv) ++{ ++ u32 rx_queues_count = priv->plat->rx_queues_to_use; ++ u32 queue; ++ u8 packet; ++ ++ for (queue = 0; queue < rx_queues_count; queue++) { ++ /* no specific packet type routing specified for the queue */ ++ if (priv->plat->rx_queues_cfg[queue].pkt_route == 0x0) ++ continue; ++ ++ packet = priv->plat->rx_queues_cfg[queue].pkt_route; ++ priv->hw->mac->rx_queue_prio(priv->hw, packet, queue); ++ } ++} ++ ++/** ++ * stmmac_mtl_configuration - Configure MTL ++ * @priv: driver private structure ++ * Description: It is used for configurring MTL ++ */ ++static void stmmac_mtl_configuration(struct stmmac_priv *priv) ++{ ++ u32 rx_queues_count = priv->plat->rx_queues_to_use; ++ u32 tx_queues_count = priv->plat->tx_queues_to_use; ++ ++ if (tx_queues_count > 1 && priv->hw->mac->set_mtl_tx_queue_weight) ++ stmmac_set_tx_queue_weight(priv); ++ ++ /* Configure MTL RX algorithms */ ++ if (rx_queues_count > 1 && priv->hw->mac->prog_mtl_rx_algorithms) ++ priv->hw->mac->prog_mtl_rx_algorithms(priv->hw, ++ priv->plat->rx_sched_algorithm); ++ ++ /* Configure MTL TX algorithms */ ++ if (tx_queues_count > 1 && priv->hw->mac->prog_mtl_tx_algorithms) ++ priv->hw->mac->prog_mtl_tx_algorithms(priv->hw, ++ priv->plat->tx_sched_algorithm); ++ ++ /* Configure CBS in AVB TX queues */ ++ if (tx_queues_count > 1 && priv->hw->mac->config_cbs) ++ stmmac_configure_cbs(priv); ++ ++ /* Map RX MTL to DMA channels */ ++ if (priv->hw->mac->map_mtl_to_dma) ++ stmmac_rx_queue_dma_chan_map(priv); ++ ++ /* Enable MAC RX Queues */ ++ if (priv->hw->mac->rx_queue_enable) ++ stmmac_mac_enable_rx_queues(priv); ++ ++ /* Set RX priorities */ ++ if (rx_queues_count > 1 && priv->hw->mac->rx_queue_prio) ++ stmmac_mac_config_rx_queues_prio(priv); ++ ++ /* Set TX priorities */ ++ if (tx_queues_count > 1 && priv->hw->mac->tx_queue_prio) ++ stmmac_mac_config_tx_queues_prio(priv); ++ ++ /* Set RX routing */ ++ if (rx_queues_count > 1 && priv->hw->mac->rx_queue_routing) ++ stmmac_mac_config_rx_queues_routing(priv); ++} ++ + /** + * stmmac_hw_setup - setup mac in a usable state. + * @dev : pointer to the device structure. +@@ -1659,6 +2430,9 @@ static void stmmac_init_tx_coalesce(stru + static int stmmac_hw_setup(struct net_device *dev, bool init_ptp) + { + struct stmmac_priv *priv = netdev_priv(dev); ++ u32 rx_cnt = priv->plat->rx_queues_to_use; ++ u32 tx_cnt = priv->plat->tx_queues_to_use; ++ u32 chan; + int ret; + + /* DMA initialization and SW reset */ +@@ -1688,9 +2462,9 @@ static int stmmac_hw_setup(struct net_de + /* Initialize the MAC Core */ + priv->hw->mac->core_init(priv->hw, dev->mtu); + +- /* Initialize MAC RX Queues */ +- if (priv->hw->mac->rx_queue_enable) +- stmmac_mac_enable_rx_queues(priv); ++ /* Initialize MTL*/ ++ if (priv->synopsys_id >= DWMAC_CORE_4_00) ++ stmmac_mtl_configuration(priv); + + ret = priv->hw->mac->rx_ipc(priv->hw); + if (!ret) { +@@ -1700,10 +2474,7 @@ static int stmmac_hw_setup(struct net_de + } + + /* Enable the MAC Rx/Tx */ +- if (priv->synopsys_id >= DWMAC_CORE_4_00) +- stmmac_dwmac4_set_mac(priv->ioaddr, true); +- else +- stmmac_set_mac(priv->ioaddr, true); ++ priv->hw->mac->set_mac(priv->ioaddr, true); + + /* Set the HW DMA mode and the COE */ + stmmac_dma_operation_mode(priv); +@@ -1711,6 +2482,10 @@ static int stmmac_hw_setup(struct net_de + stmmac_mmc_setup(priv); + + if (init_ptp) { ++ ret = clk_prepare_enable(priv->plat->clk_ptp_ref); ++ if (ret < 0) ++ netdev_warn(priv->dev, "failed to enable PTP reference clock: %d\n", ret); ++ + ret = stmmac_init_ptp(priv); + if (ret == -EOPNOTSUPP) + netdev_warn(priv->dev, "PTP not supported by HW\n"); +@@ -1725,35 +2500,37 @@ static int stmmac_hw_setup(struct net_de + __func__); + #endif + /* Start the ball rolling... */ +- netdev_dbg(priv->dev, "DMA RX/TX processes started...\n"); +- priv->hw->dma->start_tx(priv->ioaddr); +- priv->hw->dma->start_rx(priv->ioaddr); ++ stmmac_start_all_dma(priv); + + priv->tx_lpi_timer = STMMAC_DEFAULT_TWT_LS; + + if ((priv->use_riwt) && (priv->hw->dma->rx_watchdog)) { + priv->rx_riwt = MAX_DMA_RIWT; +- priv->hw->dma->rx_watchdog(priv->ioaddr, MAX_DMA_RIWT); ++ priv->hw->dma->rx_watchdog(priv->ioaddr, MAX_DMA_RIWT, rx_cnt); + } + + if (priv->hw->pcs && priv->hw->mac->pcs_ctrl_ane) + priv->hw->mac->pcs_ctrl_ane(priv->hw, 1, priv->hw->ps, 0); + +- /* set TX ring length */ +- if (priv->hw->dma->set_tx_ring_len) +- priv->hw->dma->set_tx_ring_len(priv->ioaddr, +- (DMA_TX_SIZE - 1)); +- /* set RX ring length */ +- if (priv->hw->dma->set_rx_ring_len) +- priv->hw->dma->set_rx_ring_len(priv->ioaddr, +- (DMA_RX_SIZE - 1)); ++ /* set TX and RX rings length */ ++ stmmac_set_rings_length(priv); ++ + /* Enable TSO */ +- if (priv->tso) +- priv->hw->dma->enable_tso(priv->ioaddr, 1, STMMAC_CHAN0); ++ if (priv->tso) { ++ for (chan = 0; chan < tx_cnt; chan++) ++ priv->hw->dma->enable_tso(priv->ioaddr, 1, chan); ++ } + + return 0; + } + ++static void stmmac_hw_teardown(struct net_device *dev) ++{ ++ struct stmmac_priv *priv = netdev_priv(dev); ++ ++ clk_disable_unprepare(priv->plat->clk_ptp_ref); ++} ++ + /** + * stmmac_open - open entry point of the driver + * @dev : pointer to the device structure. +@@ -1821,7 +2598,7 @@ static int stmmac_open(struct net_device + netdev_err(priv->dev, + "%s: ERROR: allocating the IRQ %d (error: %d)\n", + __func__, dev->irq, ret); +- goto init_error; ++ goto irq_error; + } + + /* Request the Wake IRQ in case of another line is used for WoL */ +@@ -1848,8 +2625,8 @@ static int stmmac_open(struct net_device + } + } + +- napi_enable(&priv->napi); +- netif_start_queue(dev); ++ stmmac_enable_all_queues(priv); ++ stmmac_start_all_queues(priv); + + return 0; + +@@ -1858,7 +2635,12 @@ lpiirq_error: + free_irq(priv->wol_irq, dev); + wolirq_error: + free_irq(dev->irq, dev); ++irq_error: ++ if (dev->phydev) ++ phy_stop(dev->phydev); + ++ del_timer_sync(&priv->txtimer); ++ stmmac_hw_teardown(dev); + init_error: + free_dma_desc_resources(priv); + dma_desc_error: +@@ -1887,9 +2669,9 @@ static int stmmac_release(struct net_dev + phy_disconnect(dev->phydev); + } + +- netif_stop_queue(dev); ++ stmmac_stop_all_queues(priv); + +- napi_disable(&priv->napi); ++ stmmac_disable_all_queues(priv); + + del_timer_sync(&priv->txtimer); + +@@ -1901,14 +2683,13 @@ static int stmmac_release(struct net_dev + free_irq(priv->lpi_irq, dev); + + /* Stop TX/RX DMA and clear the descriptors */ +- priv->hw->dma->stop_tx(priv->ioaddr); +- priv->hw->dma->stop_rx(priv->ioaddr); ++ stmmac_stop_all_dma(priv); + + /* Release and free the Rx/Tx resources */ + free_dma_desc_resources(priv); + + /* Disable the MAC Rx/Tx */ +- stmmac_set_mac(priv->ioaddr, false); ++ priv->hw->mac->set_mac(priv->ioaddr, false); + + netif_carrier_off(dev); + +@@ -1927,22 +2708,24 @@ static int stmmac_release(struct net_dev + * @des: buffer start address + * @total_len: total length to fill in descriptors + * @last_segmant: condition for the last descriptor ++ * @queue: TX queue index + * Description: + * This function fills descriptor and request new descriptors according to + * buffer length to fill + */ + static void stmmac_tso_allocator(struct stmmac_priv *priv, unsigned int des, +- int total_len, bool last_segment) ++ int total_len, bool last_segment, u32 queue) + { ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue]; + struct dma_desc *desc; +- int tmp_len; + u32 buff_size; ++ int tmp_len; + + tmp_len = total_len; + + while (tmp_len > 0) { +- priv->cur_tx = STMMAC_GET_ENTRY(priv->cur_tx, DMA_TX_SIZE); +- desc = priv->dma_tx + priv->cur_tx; ++ tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, DMA_TX_SIZE); ++ desc = tx_q->dma_tx + tx_q->cur_tx; + + desc->des0 = cpu_to_le32(des + (total_len - tmp_len)); + buff_size = tmp_len >= TSO_MAX_BUFF_SIZE ? +@@ -1950,7 +2733,7 @@ static void stmmac_tso_allocator(struct + + priv->hw->desc->prepare_tso_tx_desc(desc, 0, buff_size, + 0, 1, +- (last_segment) && (buff_size < TSO_MAX_BUFF_SIZE), ++ (last_segment) && (tmp_len <= TSO_MAX_BUFF_SIZE), + 0, 0); + + tmp_len -= TSO_MAX_BUFF_SIZE; +@@ -1986,23 +2769,28 @@ static void stmmac_tso_allocator(struct + */ + static netdev_tx_t stmmac_tso_xmit(struct sk_buff *skb, struct net_device *dev) + { +- u32 pay_len, mss; +- int tmp_pay_len = 0; ++ struct dma_desc *desc, *first, *mss_desc = NULL; + struct stmmac_priv *priv = netdev_priv(dev); + int nfrags = skb_shinfo(skb)->nr_frags; ++ u32 queue = skb_get_queue_mapping(skb); + unsigned int first_entry, des; +- struct dma_desc *desc, *first, *mss_desc = NULL; ++ struct stmmac_tx_queue *tx_q; ++ int tmp_pay_len = 0; ++ u32 pay_len, mss; + u8 proto_hdr_len; + int i; + ++ tx_q = &priv->tx_queue[queue]; ++ + /* Compute header lengths */ + proto_hdr_len = skb_transport_offset(skb) + tcp_hdrlen(skb); + + /* Desc availability based on threshold should be enough safe */ +- if (unlikely(stmmac_tx_avail(priv) < ++ if (unlikely(stmmac_tx_avail(priv, queue) < + (((skb->len - proto_hdr_len) / TSO_MAX_BUFF_SIZE + 1)))) { +- if (!netif_queue_stopped(dev)) { +- netif_stop_queue(dev); ++ if (!netif_tx_queue_stopped(netdev_get_tx_queue(dev, queue))) { ++ netif_tx_stop_queue(netdev_get_tx_queue(priv->dev, ++ queue)); + /* This is a hard error, log it. */ + netdev_err(priv->dev, + "%s: Tx Ring full when queue awake\n", +@@ -2017,10 +2805,10 @@ static netdev_tx_t stmmac_tso_xmit(struc + + /* set new MSS value if needed */ + if (mss != priv->mss) { +- mss_desc = priv->dma_tx + priv->cur_tx; ++ mss_desc = tx_q->dma_tx + tx_q->cur_tx; + priv->hw->desc->set_mss(mss_desc, mss); + priv->mss = mss; +- priv->cur_tx = STMMAC_GET_ENTRY(priv->cur_tx, DMA_TX_SIZE); ++ tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, DMA_TX_SIZE); + } + + if (netif_msg_tx_queued(priv)) { +@@ -2030,9 +2818,9 @@ static netdev_tx_t stmmac_tso_xmit(struc + skb->data_len); + } + +- first_entry = priv->cur_tx; ++ first_entry = tx_q->cur_tx; + +- desc = priv->dma_tx + first_entry; ++ desc = tx_q->dma_tx + first_entry; + first = desc; + + /* first descriptor: fill Headers on Buf1 */ +@@ -2041,9 +2829,8 @@ static netdev_tx_t stmmac_tso_xmit(struc + if (dma_mapping_error(priv->device, des)) + goto dma_map_err; + +- priv->tx_skbuff_dma[first_entry].buf = des; +- priv->tx_skbuff_dma[first_entry].len = skb_headlen(skb); +- priv->tx_skbuff[first_entry] = skb; ++ tx_q->tx_skbuff_dma[first_entry].buf = des; ++ tx_q->tx_skbuff_dma[first_entry].len = skb_headlen(skb); + + first->des0 = cpu_to_le32(des); + +@@ -2054,7 +2841,7 @@ static netdev_tx_t stmmac_tso_xmit(struc + /* If needed take extra descriptors to fill the remaining payload */ + tmp_pay_len = pay_len - TSO_MAX_BUFF_SIZE; + +- stmmac_tso_allocator(priv, des, tmp_pay_len, (nfrags == 0)); ++ stmmac_tso_allocator(priv, des, tmp_pay_len, (nfrags == 0), queue); + + /* Prepare fragments */ + for (i = 0; i < nfrags; i++) { +@@ -2063,24 +2850,34 @@ static netdev_tx_t stmmac_tso_xmit(struc + des = skb_frag_dma_map(priv->device, frag, 0, + skb_frag_size(frag), + DMA_TO_DEVICE); ++ if (dma_mapping_error(priv->device, des)) ++ goto dma_map_err; + + stmmac_tso_allocator(priv, des, skb_frag_size(frag), +- (i == nfrags - 1)); ++ (i == nfrags - 1), queue); + +- priv->tx_skbuff_dma[priv->cur_tx].buf = des; +- priv->tx_skbuff_dma[priv->cur_tx].len = skb_frag_size(frag); +- priv->tx_skbuff[priv->cur_tx] = NULL; +- priv->tx_skbuff_dma[priv->cur_tx].map_as_page = true; ++ tx_q->tx_skbuff_dma[tx_q->cur_tx].buf = des; ++ tx_q->tx_skbuff_dma[tx_q->cur_tx].len = skb_frag_size(frag); ++ tx_q->tx_skbuff[tx_q->cur_tx] = NULL; ++ tx_q->tx_skbuff_dma[tx_q->cur_tx].map_as_page = true; + } + +- priv->tx_skbuff_dma[priv->cur_tx].last_segment = true; ++ tx_q->tx_skbuff_dma[tx_q->cur_tx].last_segment = true; ++ ++ /* Only the last descriptor gets to point to the skb. */ ++ tx_q->tx_skbuff[tx_q->cur_tx] = skb; + +- priv->cur_tx = STMMAC_GET_ENTRY(priv->cur_tx, DMA_TX_SIZE); ++ /* We've used all descriptors we need for this skb, however, ++ * advance cur_tx so that it references a fresh descriptor. ++ * ndo_start_xmit will fill this descriptor the next time it's ++ * called and stmmac_tx_clean may clean up to this descriptor. ++ */ ++ tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, DMA_TX_SIZE); + +- if (unlikely(stmmac_tx_avail(priv) <= (MAX_SKB_FRAGS + 1))) { ++ if (unlikely(stmmac_tx_avail(priv, queue) <= (MAX_SKB_FRAGS + 1))) { + netif_dbg(priv, hw, priv->dev, "%s: stop transmitted packets\n", + __func__); +- netif_stop_queue(dev); ++ netif_tx_stop_queue(netdev_get_tx_queue(priv->dev, queue)); + } + + dev->stats.tx_bytes += skb->len; +@@ -2112,7 +2909,7 @@ static netdev_tx_t stmmac_tso_xmit(struc + priv->hw->desc->prepare_tso_tx_desc(first, 1, + proto_hdr_len, + pay_len, +- 1, priv->tx_skbuff_dma[first_entry].last_segment, ++ 1, tx_q->tx_skbuff_dma[first_entry].last_segment, + tcp_hdrlen(skb) / 4, (skb->len - proto_hdr_len)); + + /* If context desc is used to change MSS */ +@@ -2127,20 +2924,20 @@ static netdev_tx_t stmmac_tso_xmit(struc + + if (netif_msg_pktdata(priv)) { + pr_info("%s: curr=%d dirty=%d f=%d, e=%d, f_p=%p, nfrags %d\n", +- __func__, priv->cur_tx, priv->dirty_tx, first_entry, +- priv->cur_tx, first, nfrags); ++ __func__, tx_q->cur_tx, tx_q->dirty_tx, first_entry, ++ tx_q->cur_tx, first, nfrags); + +- priv->hw->desc->display_ring((void *)priv->dma_tx, DMA_TX_SIZE, ++ priv->hw->desc->display_ring((void *)tx_q->dma_tx, DMA_TX_SIZE, + 0); + + pr_info(">>> frame to be transmitted: "); + print_pkt(skb->data, skb_headlen(skb)); + } + +- netdev_sent_queue(dev, skb->len); ++ netdev_tx_sent_queue(netdev_get_tx_queue(dev, queue), skb->len); + +- priv->hw->dma->set_tx_tail_ptr(priv->ioaddr, priv->tx_tail_addr, +- STMMAC_CHAN0); ++ priv->hw->dma->set_tx_tail_ptr(priv->ioaddr, tx_q->tx_tail_addr, ++ queue); + + return NETDEV_TX_OK; + +@@ -2164,21 +2961,27 @@ static netdev_tx_t stmmac_xmit(struct sk + struct stmmac_priv *priv = netdev_priv(dev); + unsigned int nopaged_len = skb_headlen(skb); + int i, csum_insertion = 0, is_jumbo = 0; ++ u32 queue = skb_get_queue_mapping(skb); + int nfrags = skb_shinfo(skb)->nr_frags; +- unsigned int entry, first_entry; ++ int entry; ++ unsigned int first_entry; + struct dma_desc *desc, *first; ++ struct stmmac_tx_queue *tx_q; + unsigned int enh_desc; + unsigned int des; + ++ tx_q = &priv->tx_queue[queue]; ++ + /* Manage oversized TCP frames for GMAC4 device */ + if (skb_is_gso(skb) && priv->tso) { + if (ip_hdr(skb)->protocol == IPPROTO_TCP) + return stmmac_tso_xmit(skb, dev); + } + +- if (unlikely(stmmac_tx_avail(priv) < nfrags + 1)) { +- if (!netif_queue_stopped(dev)) { +- netif_stop_queue(dev); ++ if (unlikely(stmmac_tx_avail(priv, queue) < nfrags + 1)) { ++ if (!netif_tx_queue_stopped(netdev_get_tx_queue(dev, queue))) { ++ netif_tx_stop_queue(netdev_get_tx_queue(priv->dev, ++ queue)); + /* This is a hard error, log it. */ + netdev_err(priv->dev, + "%s: Tx Ring full when queue awake\n", +@@ -2190,20 +2993,18 @@ static netdev_tx_t stmmac_xmit(struct sk + if (priv->tx_path_in_lpi_mode) + stmmac_disable_eee_mode(priv); + +- entry = priv->cur_tx; ++ entry = tx_q->cur_tx; + first_entry = entry; + + csum_insertion = (skb->ip_summed == CHECKSUM_PARTIAL); + + if (likely(priv->extend_desc)) +- desc = (struct dma_desc *)(priv->dma_etx + entry); ++ desc = (struct dma_desc *)(tx_q->dma_etx + entry); + else +- desc = priv->dma_tx + entry; ++ desc = tx_q->dma_tx + entry; + + first = desc; + +- priv->tx_skbuff[first_entry] = skb; +- + enh_desc = priv->plat->enh_desc; + /* To program the descriptors according to the size of the frame */ + if (enh_desc) +@@ -2211,7 +3012,7 @@ static netdev_tx_t stmmac_xmit(struct sk + + if (unlikely(is_jumbo) && likely(priv->synopsys_id < + DWMAC_CORE_4_00)) { +- entry = priv->hw->mode->jumbo_frm(priv, skb, csum_insertion); ++ entry = priv->hw->mode->jumbo_frm(tx_q, skb, csum_insertion); + if (unlikely(entry < 0)) + goto dma_map_err; + } +@@ -2224,48 +3025,56 @@ static netdev_tx_t stmmac_xmit(struct sk + entry = STMMAC_GET_ENTRY(entry, DMA_TX_SIZE); + + if (likely(priv->extend_desc)) +- desc = (struct dma_desc *)(priv->dma_etx + entry); ++ desc = (struct dma_desc *)(tx_q->dma_etx + entry); + else +- desc = priv->dma_tx + entry; ++ desc = tx_q->dma_tx + entry; + + des = skb_frag_dma_map(priv->device, frag, 0, len, + DMA_TO_DEVICE); + if (dma_mapping_error(priv->device, des)) + goto dma_map_err; /* should reuse desc w/o issues */ + +- priv->tx_skbuff[entry] = NULL; ++ tx_q->tx_skbuff[entry] = NULL; + +- priv->tx_skbuff_dma[entry].buf = des; ++ tx_q->tx_skbuff_dma[entry].buf = des; + if (unlikely(priv->synopsys_id >= DWMAC_CORE_4_00)) + desc->des0 = cpu_to_le32(des); + else + desc->des2 = cpu_to_le32(des); + +- priv->tx_skbuff_dma[entry].map_as_page = true; +- priv->tx_skbuff_dma[entry].len = len; +- priv->tx_skbuff_dma[entry].last_segment = last_segment; ++ tx_q->tx_skbuff_dma[entry].map_as_page = true; ++ tx_q->tx_skbuff_dma[entry].len = len; ++ tx_q->tx_skbuff_dma[entry].last_segment = last_segment; + + /* Prepare the descriptor and set the own bit too */ + priv->hw->desc->prepare_tx_desc(desc, 0, len, csum_insertion, +- priv->mode, 1, last_segment); ++ priv->mode, 1, last_segment, ++ skb->len); + } + +- entry = STMMAC_GET_ENTRY(entry, DMA_TX_SIZE); ++ /* Only the last descriptor gets to point to the skb. */ ++ tx_q->tx_skbuff[entry] = skb; + +- priv->cur_tx = entry; ++ /* We've used all descriptors we need for this skb, however, ++ * advance cur_tx so that it references a fresh descriptor. ++ * ndo_start_xmit will fill this descriptor the next time it's ++ * called and stmmac_tx_clean may clean up to this descriptor. ++ */ ++ entry = STMMAC_GET_ENTRY(entry, DMA_TX_SIZE); ++ tx_q->cur_tx = entry; + + if (netif_msg_pktdata(priv)) { + void *tx_head; + + netdev_dbg(priv->dev, + "%s: curr=%d dirty=%d f=%d, e=%d, first=%p, nfrags=%d", +- __func__, priv->cur_tx, priv->dirty_tx, first_entry, ++ __func__, tx_q->cur_tx, tx_q->dirty_tx, first_entry, + entry, first, nfrags); + + if (priv->extend_desc) +- tx_head = (void *)priv->dma_etx; ++ tx_head = (void *)tx_q->dma_etx; + else +- tx_head = (void *)priv->dma_tx; ++ tx_head = (void *)tx_q->dma_tx; + + priv->hw->desc->display_ring(tx_head, DMA_TX_SIZE, false); + +@@ -2273,10 +3082,10 @@ static netdev_tx_t stmmac_xmit(struct sk + print_pkt(skb->data, skb->len); + } + +- if (unlikely(stmmac_tx_avail(priv) <= (MAX_SKB_FRAGS + 1))) { ++ if (unlikely(stmmac_tx_avail(priv, queue) <= (MAX_SKB_FRAGS + 1))) { + netif_dbg(priv, hw, priv->dev, "%s: stop transmitted packets\n", + __func__); +- netif_stop_queue(dev); ++ netif_tx_stop_queue(netdev_get_tx_queue(priv->dev, queue)); + } + + dev->stats.tx_bytes += skb->len; +@@ -2311,14 +3120,14 @@ static netdev_tx_t stmmac_xmit(struct sk + if (dma_mapping_error(priv->device, des)) + goto dma_map_err; + +- priv->tx_skbuff_dma[first_entry].buf = des; ++ tx_q->tx_skbuff_dma[first_entry].buf = des; + if (unlikely(priv->synopsys_id >= DWMAC_CORE_4_00)) + first->des0 = cpu_to_le32(des); + else + first->des2 = cpu_to_le32(des); + +- priv->tx_skbuff_dma[first_entry].len = nopaged_len; +- priv->tx_skbuff_dma[first_entry].last_segment = last_segment; ++ tx_q->tx_skbuff_dma[first_entry].len = nopaged_len; ++ tx_q->tx_skbuff_dma[first_entry].last_segment = last_segment; + + if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) && + priv->hwts_tx_en)) { +@@ -2330,7 +3139,7 @@ static netdev_tx_t stmmac_xmit(struct sk + /* Prepare the first descriptor setting the OWN bit too */ + priv->hw->desc->prepare_tx_desc(first, 1, nopaged_len, + csum_insertion, priv->mode, 1, +- last_segment); ++ last_segment, skb->len); + + /* The own bit must be the latest setting done when prepare the + * descriptor and then barrier is needed to make sure that +@@ -2339,13 +3148,13 @@ static netdev_tx_t stmmac_xmit(struct sk + dma_wmb(); + } + +- netdev_sent_queue(dev, skb->len); ++ netdev_tx_sent_queue(netdev_get_tx_queue(dev, queue), skb->len); + + if (priv->synopsys_id < DWMAC_CORE_4_00) + priv->hw->dma->enable_dma_transmission(priv->ioaddr); + else +- priv->hw->dma->set_tx_tail_ptr(priv->ioaddr, priv->tx_tail_addr, +- STMMAC_CHAN0); ++ priv->hw->dma->set_tx_tail_ptr(priv->ioaddr, tx_q->tx_tail_addr, ++ queue); + + return NETDEV_TX_OK; + +@@ -2373,9 +3182,9 @@ static void stmmac_rx_vlan(struct net_de + } + + +-static inline int stmmac_rx_threshold_count(struct stmmac_priv *priv) ++static inline int stmmac_rx_threshold_count(struct stmmac_rx_queue *rx_q) + { +- if (priv->rx_zeroc_thresh < STMMAC_RX_THRESH) ++ if (rx_q->rx_zeroc_thresh < STMMAC_RX_THRESH) + return 0; + + return 1; +@@ -2384,30 +3193,33 @@ static inline int stmmac_rx_threshold_co + /** + * stmmac_rx_refill - refill used skb preallocated buffers + * @priv: driver private structure ++ * @queue: RX queue index + * Description : this is to reallocate the skb for the reception process + * that is based on zero-copy. + */ +-static inline void stmmac_rx_refill(struct stmmac_priv *priv) ++static inline void stmmac_rx_refill(struct stmmac_priv *priv, u32 queue) + { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; ++ int dirty = stmmac_rx_dirty(priv, queue); ++ unsigned int entry = rx_q->dirty_rx; ++ + int bfsize = priv->dma_buf_sz; +- unsigned int entry = priv->dirty_rx; +- int dirty = stmmac_rx_dirty(priv); + + while (dirty-- > 0) { + struct dma_desc *p; + + if (priv->extend_desc) +- p = (struct dma_desc *)(priv->dma_erx + entry); ++ p = (struct dma_desc *)(rx_q->dma_erx + entry); + else +- p = priv->dma_rx + entry; ++ p = rx_q->dma_rx + entry; + +- if (likely(priv->rx_skbuff[entry] == NULL)) { ++ if (likely(!rx_q->rx_skbuff[entry])) { + struct sk_buff *skb; + + skb = netdev_alloc_skb_ip_align(priv->dev, bfsize); + if (unlikely(!skb)) { + /* so for a while no zero-copy! */ +- priv->rx_zeroc_thresh = STMMAC_RX_THRESH; ++ rx_q->rx_zeroc_thresh = STMMAC_RX_THRESH; + if (unlikely(net_ratelimit())) + dev_err(priv->device, + "fail to alloc skb entry %d\n", +@@ -2415,28 +3227,28 @@ static inline void stmmac_rx_refill(stru + break; + } + +- priv->rx_skbuff[entry] = skb; +- priv->rx_skbuff_dma[entry] = ++ rx_q->rx_skbuff[entry] = skb; ++ rx_q->rx_skbuff_dma[entry] = + dma_map_single(priv->device, skb->data, bfsize, + DMA_FROM_DEVICE); + if (dma_mapping_error(priv->device, +- priv->rx_skbuff_dma[entry])) { ++ rx_q->rx_skbuff_dma[entry])) { + netdev_err(priv->dev, "Rx DMA map failed\n"); + dev_kfree_skb(skb); + break; + } + + if (unlikely(priv->synopsys_id >= DWMAC_CORE_4_00)) { +- p->des0 = cpu_to_le32(priv->rx_skbuff_dma[entry]); ++ p->des0 = cpu_to_le32(rx_q->rx_skbuff_dma[entry]); + p->des1 = 0; + } else { +- p->des2 = cpu_to_le32(priv->rx_skbuff_dma[entry]); ++ p->des2 = cpu_to_le32(rx_q->rx_skbuff_dma[entry]); + } + if (priv->hw->mode->refill_desc3) +- priv->hw->mode->refill_desc3(priv, p); ++ priv->hw->mode->refill_desc3(rx_q, p); + +- if (priv->rx_zeroc_thresh > 0) +- priv->rx_zeroc_thresh--; ++ if (rx_q->rx_zeroc_thresh > 0) ++ rx_q->rx_zeroc_thresh--; + + netif_dbg(priv, rx_status, priv->dev, + "refill entry #%d\n", entry); +@@ -2452,31 +3264,33 @@ static inline void stmmac_rx_refill(stru + + entry = STMMAC_GET_ENTRY(entry, DMA_RX_SIZE); + } +- priv->dirty_rx = entry; ++ rx_q->dirty_rx = entry; + } + + /** + * stmmac_rx - manage the receive process + * @priv: driver private structure +- * @limit: napi bugget. ++ * @limit: napi bugget ++ * @queue: RX queue index. + * Description : this the function called by the napi poll method. + * It gets all the frames inside the ring. + */ +-static int stmmac_rx(struct stmmac_priv *priv, int limit) ++static int stmmac_rx(struct stmmac_priv *priv, int limit, u32 queue) + { +- unsigned int entry = priv->cur_rx; ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; ++ unsigned int entry = rx_q->cur_rx; ++ int coe = priv->hw->rx_csum; + unsigned int next_entry; + unsigned int count = 0; +- int coe = priv->hw->rx_csum; + + if (netif_msg_rx_status(priv)) { + void *rx_head; + + netdev_dbg(priv->dev, "%s: descriptor ring:\n", __func__); + if (priv->extend_desc) +- rx_head = (void *)priv->dma_erx; ++ rx_head = (void *)rx_q->dma_erx; + else +- rx_head = (void *)priv->dma_rx; ++ rx_head = (void *)rx_q->dma_rx; + + priv->hw->desc->display_ring(rx_head, DMA_RX_SIZE, true); + } +@@ -2486,9 +3300,9 @@ static int stmmac_rx(struct stmmac_priv + struct dma_desc *np; + + if (priv->extend_desc) +- p = (struct dma_desc *)(priv->dma_erx + entry); ++ p = (struct dma_desc *)(rx_q->dma_erx + entry); + else +- p = priv->dma_rx + entry; ++ p = rx_q->dma_rx + entry; + + /* read the status of the incoming frame */ + status = priv->hw->desc->rx_status(&priv->dev->stats, +@@ -2499,20 +3313,20 @@ static int stmmac_rx(struct stmmac_priv + + count++; + +- priv->cur_rx = STMMAC_GET_ENTRY(priv->cur_rx, DMA_RX_SIZE); +- next_entry = priv->cur_rx; ++ rx_q->cur_rx = STMMAC_GET_ENTRY(rx_q->cur_rx, DMA_RX_SIZE); ++ next_entry = rx_q->cur_rx; + + if (priv->extend_desc) +- np = (struct dma_desc *)(priv->dma_erx + next_entry); ++ np = (struct dma_desc *)(rx_q->dma_erx + next_entry); + else +- np = priv->dma_rx + next_entry; ++ np = rx_q->dma_rx + next_entry; + + prefetch(np); + + if ((priv->extend_desc) && (priv->hw->desc->rx_extended_status)) + priv->hw->desc->rx_extended_status(&priv->dev->stats, + &priv->xstats, +- priv->dma_erx + ++ rx_q->dma_erx + + entry); + if (unlikely(status == discard_frame)) { + priv->dev->stats.rx_errors++; +@@ -2522,9 +3336,9 @@ static int stmmac_rx(struct stmmac_priv + * them in stmmac_rx_refill() function so that + * device can reuse it. + */ +- priv->rx_skbuff[entry] = NULL; ++ rx_q->rx_skbuff[entry] = NULL; + dma_unmap_single(priv->device, +- priv->rx_skbuff_dma[entry], ++ rx_q->rx_skbuff_dma[entry], + priv->dma_buf_sz, + DMA_FROM_DEVICE); + } +@@ -2572,7 +3386,7 @@ static int stmmac_rx(struct stmmac_priv + */ + if (unlikely(!priv->plat->has_gmac4 && + ((frame_len < priv->rx_copybreak) || +- stmmac_rx_threshold_count(priv)))) { ++ stmmac_rx_threshold_count(rx_q)))) { + skb = netdev_alloc_skb_ip_align(priv->dev, + frame_len); + if (unlikely(!skb)) { +@@ -2584,21 +3398,21 @@ static int stmmac_rx(struct stmmac_priv + } + + dma_sync_single_for_cpu(priv->device, +- priv->rx_skbuff_dma ++ rx_q->rx_skbuff_dma + [entry], frame_len, + DMA_FROM_DEVICE); + skb_copy_to_linear_data(skb, +- priv-> ++ rx_q-> + rx_skbuff[entry]->data, + frame_len); + + skb_put(skb, frame_len); + dma_sync_single_for_device(priv->device, +- priv->rx_skbuff_dma ++ rx_q->rx_skbuff_dma + [entry], frame_len, + DMA_FROM_DEVICE); + } else { +- skb = priv->rx_skbuff[entry]; ++ skb = rx_q->rx_skbuff[entry]; + if (unlikely(!skb)) { + netdev_err(priv->dev, + "%s: Inconsistent Rx chain\n", +@@ -2607,12 +3421,12 @@ static int stmmac_rx(struct stmmac_priv + break; + } + prefetch(skb->data - NET_IP_ALIGN); +- priv->rx_skbuff[entry] = NULL; +- priv->rx_zeroc_thresh++; ++ rx_q->rx_skbuff[entry] = NULL; ++ rx_q->rx_zeroc_thresh++; + + skb_put(skb, frame_len); + dma_unmap_single(priv->device, +- priv->rx_skbuff_dma[entry], ++ rx_q->rx_skbuff_dma[entry], + priv->dma_buf_sz, + DMA_FROM_DEVICE); + } +@@ -2634,7 +3448,7 @@ static int stmmac_rx(struct stmmac_priv + else + skb->ip_summed = CHECKSUM_UNNECESSARY; + +- napi_gro_receive(&priv->napi, skb); ++ napi_gro_receive(&rx_q->napi, skb); + + priv->dev->stats.rx_packets++; + priv->dev->stats.rx_bytes += frame_len; +@@ -2642,7 +3456,7 @@ static int stmmac_rx(struct stmmac_priv + entry = next_entry; + } + +- stmmac_rx_refill(priv); ++ stmmac_rx_refill(priv, queue); + + priv->xstats.rx_pkt_n += count; + +@@ -2659,16 +3473,24 @@ static int stmmac_rx(struct stmmac_priv + */ + static int stmmac_poll(struct napi_struct *napi, int budget) + { +- struct stmmac_priv *priv = container_of(napi, struct stmmac_priv, napi); ++ struct stmmac_rx_queue *rx_q = ++ container_of(napi, struct stmmac_rx_queue, napi); ++ struct stmmac_priv *priv = rx_q->priv_data; ++ u32 tx_count = priv->plat->tx_queues_to_use; ++ u32 chan = rx_q->queue_index; + int work_done = 0; ++ u32 queue; + + priv->xstats.napi_poll++; +- stmmac_tx_clean(priv); + +- work_done = stmmac_rx(priv, budget); ++ /* check all the queues */ ++ for (queue = 0; queue < tx_count; queue++) ++ stmmac_tx_clean(priv, queue); ++ ++ work_done = stmmac_rx(priv, budget, rx_q->queue_index); + if (work_done < budget) { + napi_complete_done(napi, work_done); +- stmmac_enable_dma_irq(priv); ++ stmmac_enable_dma_irq(priv, chan); + } + return work_done; + } +@@ -2684,9 +3506,12 @@ static int stmmac_poll(struct napi_struc + static void stmmac_tx_timeout(struct net_device *dev) + { + struct stmmac_priv *priv = netdev_priv(dev); ++ u32 tx_count = priv->plat->tx_queues_to_use; ++ u32 chan; + + /* Clear Tx resources and restart transmitting again */ +- stmmac_tx_err(priv); ++ for (chan = 0; chan < tx_count; chan++) ++ stmmac_tx_err(priv, chan); + } + + /** +@@ -2809,6 +3634,12 @@ static irqreturn_t stmmac_interrupt(int + { + struct net_device *dev = (struct net_device *)dev_id; + struct stmmac_priv *priv = netdev_priv(dev); ++ u32 rx_cnt = priv->plat->rx_queues_to_use; ++ u32 tx_cnt = priv->plat->tx_queues_to_use; ++ u32 queues_count; ++ u32 queue; ++ ++ queues_count = (rx_cnt > tx_cnt) ? rx_cnt : tx_cnt; + + if (priv->irq_wake) + pm_wakeup_event(priv->device, 0); +@@ -2822,16 +3653,30 @@ static irqreturn_t stmmac_interrupt(int + if ((priv->plat->has_gmac) || (priv->plat->has_gmac4)) { + int status = priv->hw->mac->host_irq_status(priv->hw, + &priv->xstats); ++ + if (unlikely(status)) { + /* For LPI we need to save the tx status */ + if (status & CORE_IRQ_TX_PATH_IN_LPI_MODE) + priv->tx_path_in_lpi_mode = true; + if (status & CORE_IRQ_TX_PATH_EXIT_LPI_MODE) + priv->tx_path_in_lpi_mode = false; +- if (status & CORE_IRQ_MTL_RX_OVERFLOW && priv->hw->dma->set_rx_tail_ptr) +- priv->hw->dma->set_rx_tail_ptr(priv->ioaddr, +- priv->rx_tail_addr, +- STMMAC_CHAN0); ++ } ++ ++ if (priv->synopsys_id >= DWMAC_CORE_4_00) { ++ for (queue = 0; queue < queues_count; queue++) { ++ struct stmmac_rx_queue *rx_q = ++ &priv->rx_queue[queue]; ++ ++ status |= ++ priv->hw->mac->host_mtl_irq_status(priv->hw, ++ queue); ++ ++ if (status & CORE_IRQ_MTL_RX_OVERFLOW && ++ priv->hw->dma->set_rx_tail_ptr) ++ priv->hw->dma->set_rx_tail_ptr(priv->ioaddr, ++ rx_q->rx_tail_addr, ++ queue); ++ } + } + + /* PCS link status */ +@@ -2916,7 +3761,7 @@ static void sysfs_display_ring(void *hea + ep++; + } else { + seq_printf(seq, "%d [0x%x]: 0x%x 0x%x 0x%x 0x%x\n", +- i, (unsigned int)virt_to_phys(ep), ++ i, (unsigned int)virt_to_phys(p), + le32_to_cpu(p->des0), le32_to_cpu(p->des1), + le32_to_cpu(p->des2), le32_to_cpu(p->des3)); + p++; +@@ -2929,17 +3774,40 @@ static int stmmac_sysfs_ring_read(struct + { + struct net_device *dev = seq->private; + struct stmmac_priv *priv = netdev_priv(dev); ++ u32 rx_count = priv->plat->rx_queues_to_use; ++ u32 tx_count = priv->plat->tx_queues_to_use; ++ u32 queue; + +- if (priv->extend_desc) { +- seq_printf(seq, "Extended RX descriptor ring:\n"); +- sysfs_display_ring((void *)priv->dma_erx, DMA_RX_SIZE, 1, seq); +- seq_printf(seq, "Extended TX descriptor ring:\n"); +- sysfs_display_ring((void *)priv->dma_etx, DMA_TX_SIZE, 1, seq); +- } else { +- seq_printf(seq, "RX descriptor ring:\n"); +- sysfs_display_ring((void *)priv->dma_rx, DMA_RX_SIZE, 0, seq); +- seq_printf(seq, "TX descriptor ring:\n"); +- sysfs_display_ring((void *)priv->dma_tx, DMA_TX_SIZE, 0, seq); ++ for (queue = 0; queue < rx_count; queue++) { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; ++ ++ seq_printf(seq, "RX Queue %d:\n", queue); ++ ++ if (priv->extend_desc) { ++ seq_printf(seq, "Extended descriptor ring:\n"); ++ sysfs_display_ring((void *)rx_q->dma_erx, ++ DMA_RX_SIZE, 1, seq); ++ } else { ++ seq_printf(seq, "Descriptor ring:\n"); ++ sysfs_display_ring((void *)rx_q->dma_rx, ++ DMA_RX_SIZE, 0, seq); ++ } ++ } ++ ++ for (queue = 0; queue < tx_count; queue++) { ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue]; ++ ++ seq_printf(seq, "TX Queue %d:\n", queue); ++ ++ if (priv->extend_desc) { ++ seq_printf(seq, "Extended descriptor ring:\n"); ++ sysfs_display_ring((void *)tx_q->dma_etx, ++ DMA_TX_SIZE, 1, seq); ++ } else { ++ seq_printf(seq, "Descriptor ring:\n"); ++ sysfs_display_ring((void *)tx_q->dma_tx, ++ DMA_TX_SIZE, 0, seq); ++ } + } + + return 0; +@@ -3222,11 +4090,14 @@ int stmmac_dvr_probe(struct device *devi + struct plat_stmmacenet_data *plat_dat, + struct stmmac_resources *res) + { +- int ret = 0; + struct net_device *ndev = NULL; + struct stmmac_priv *priv; ++ int ret = 0; ++ u32 queue; + +- ndev = alloc_etherdev(sizeof(struct stmmac_priv)); ++ ndev = alloc_etherdev_mqs(sizeof(struct stmmac_priv), ++ MTL_MAX_TX_QUEUES, ++ MTL_MAX_RX_QUEUES); + if (!ndev) + return -ENOMEM; + +@@ -3268,6 +4139,10 @@ int stmmac_dvr_probe(struct device *devi + if (ret) + goto error_hw_init; + ++ /* Configure real RX and TX queues */ ++ netif_set_real_num_rx_queues(ndev, priv->plat->rx_queues_to_use); ++ netif_set_real_num_tx_queues(ndev, priv->plat->tx_queues_to_use); ++ + ndev->netdev_ops = &stmmac_netdev_ops; + + ndev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM | +@@ -3300,7 +4175,12 @@ int stmmac_dvr_probe(struct device *devi + "Enable RX Mitigation via HW Watchdog Timer\n"); + } + +- netif_napi_add(ndev, &priv->napi, stmmac_poll, 64); ++ for (queue = 0; queue < priv->plat->rx_queues_to_use; queue++) { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; ++ ++ netif_napi_add(ndev, &rx_q->napi, stmmac_poll, ++ (8 * priv->plat->rx_queues_to_use)); ++ } + + spin_lock_init(&priv->lock); + +@@ -3345,7 +4225,11 @@ error_netdev_register: + priv->hw->pcs != STMMAC_PCS_RTBI) + stmmac_mdio_unregister(ndev); + error_mdio_register: +- netif_napi_del(&priv->napi); ++ for (queue = 0; queue < priv->plat->rx_queues_to_use; queue++) { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; ++ ++ netif_napi_del(&rx_q->napi); ++ } + error_hw_init: + free_netdev(ndev); + +@@ -3366,10 +4250,9 @@ int stmmac_dvr_remove(struct device *dev + + netdev_info(priv->dev, "%s: removing driver", __func__); + +- priv->hw->dma->stop_rx(priv->ioaddr); +- priv->hw->dma->stop_tx(priv->ioaddr); ++ stmmac_stop_all_dma(priv); + +- stmmac_set_mac(priv->ioaddr, false); ++ priv->hw->mac->set_mac(priv->ioaddr, false); + netif_carrier_off(ndev); + unregister_netdev(ndev); + if (priv->plat->stmmac_rst) +@@ -3408,20 +4291,19 @@ int stmmac_suspend(struct device *dev) + spin_lock_irqsave(&priv->lock, flags); + + netif_device_detach(ndev); +- netif_stop_queue(ndev); ++ stmmac_stop_all_queues(priv); + +- napi_disable(&priv->napi); ++ stmmac_disable_all_queues(priv); + + /* Stop TX/RX DMA */ +- priv->hw->dma->stop_tx(priv->ioaddr); +- priv->hw->dma->stop_rx(priv->ioaddr); ++ stmmac_stop_all_dma(priv); + + /* Enable Power down mode by programming the PMT regs */ + if (device_may_wakeup(priv->device)) { + priv->hw->mac->pmt(priv->hw, priv->wolopts); + priv->irq_wake = 1; + } else { +- stmmac_set_mac(priv->ioaddr, false); ++ priv->hw->mac->set_mac(priv->ioaddr, false); + pinctrl_pm_select_sleep_state(priv->device); + /* Disable clock in case of PWM is off */ + clk_disable(priv->plat->pclk); +@@ -3437,6 +4319,31 @@ int stmmac_suspend(struct device *dev) + EXPORT_SYMBOL_GPL(stmmac_suspend); + + /** ++ * stmmac_reset_queues_param - reset queue parameters ++ * @dev: device pointer ++ */ ++static void stmmac_reset_queues_param(struct stmmac_priv *priv) ++{ ++ u32 rx_cnt = priv->plat->rx_queues_to_use; ++ u32 tx_cnt = priv->plat->tx_queues_to_use; ++ u32 queue; ++ ++ for (queue = 0; queue < rx_cnt; queue++) { ++ struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue]; ++ ++ rx_q->cur_rx = 0; ++ rx_q->dirty_rx = 0; ++ } ++ ++ for (queue = 0; queue < tx_cnt; queue++) { ++ struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue]; ++ ++ tx_q->cur_tx = 0; ++ tx_q->dirty_tx = 0; ++ } ++} ++ ++/** + * stmmac_resume - resume callback + * @dev: device pointer + * Description: when resume this function is invoked to setup the DMA and CORE +@@ -3476,10 +4383,8 @@ int stmmac_resume(struct device *dev) + + spin_lock_irqsave(&priv->lock, flags); + +- priv->cur_rx = 0; +- priv->dirty_rx = 0; +- priv->dirty_tx = 0; +- priv->cur_tx = 0; ++ stmmac_reset_queues_param(priv); ++ + /* reset private mss value to force mss context settings at + * next tso xmit (only used for gmac4). + */ +@@ -3491,9 +4396,9 @@ int stmmac_resume(struct device *dev) + stmmac_init_tx_coalesce(priv); + stmmac_set_rx_mode(ndev); + +- napi_enable(&priv->napi); ++ stmmac_enable_all_queues(priv); + +- netif_start_queue(ndev); ++ stmmac_start_all_queues(priv); + + spin_unlock_irqrestore(&priv->lock, flags); + +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c +@@ -32,6 +32,7 @@ + */ + struct stmmac_pci_dmi_data { + const char *name; ++ const char *asset_tag; + unsigned int func; + int phy_addr; + }; +@@ -46,6 +47,7 @@ struct stmmac_pci_info { + static int stmmac_pci_find_phy_addr(struct stmmac_pci_info *info) + { + const char *name = dmi_get_system_info(DMI_BOARD_NAME); ++ const char *asset_tag = dmi_get_system_info(DMI_BOARD_ASSET_TAG); + unsigned int func = PCI_FUNC(info->pdev->devfn); + struct stmmac_pci_dmi_data *dmi; + +@@ -57,18 +59,19 @@ static int stmmac_pci_find_phy_addr(stru + return 1; + + for (dmi = info->dmi; dmi->name && *dmi->name; dmi++) { +- if (!strcmp(dmi->name, name) && dmi->func == func) ++ if (!strcmp(dmi->name, name) && dmi->func == func) { ++ /* If asset tag is provided, match on it as well. */ ++ if (dmi->asset_tag && strcmp(dmi->asset_tag, asset_tag)) ++ continue; + return dmi->phy_addr; ++ } + } + + return -ENODEV; + } + +-static void stmmac_default_data(struct plat_stmmacenet_data *plat) ++static void common_default_data(struct plat_stmmacenet_data *plat) + { +- plat->bus_id = 1; +- plat->phy_addr = 0; +- plat->interface = PHY_INTERFACE_MODE_GMII; + plat->clk_csr = 2; /* clk_csr_i = 20-35MHz & MDC = clk_csr_i/16 */ + plat->has_gmac = 1; + plat->force_sf_dma_mode = 1; +@@ -76,10 +79,6 @@ static void stmmac_default_data(struct p + plat->mdio_bus_data->phy_reset = NULL; + plat->mdio_bus_data->phy_mask = 0; + +- plat->dma_cfg->pbl = 32; +- plat->dma_cfg->pblx8 = true; +- /* TODO: AXI */ +- + /* Set default value for multicast hash bins */ + plat->multicast_filter_bins = HASH_TABLE_SIZE; + +@@ -88,6 +87,31 @@ static void stmmac_default_data(struct p + + /* Set the maxmtu to a default of JUMBO_LEN */ + plat->maxmtu = JUMBO_LEN; ++ ++ /* Set default number of RX and TX queues to use */ ++ plat->tx_queues_to_use = 1; ++ plat->rx_queues_to_use = 1; ++ ++ /* Disable Priority config by default */ ++ plat->tx_queues_cfg[0].use_prio = false; ++ plat->rx_queues_cfg[0].use_prio = false; ++ ++ /* Disable RX queues routing by default */ ++ plat->rx_queues_cfg[0].pkt_route = 0x0; ++} ++ ++static void stmmac_default_data(struct plat_stmmacenet_data *plat) ++{ ++ /* Set common default data first */ ++ common_default_data(plat); ++ ++ plat->bus_id = 1; ++ plat->phy_addr = 0; ++ plat->interface = PHY_INTERFACE_MODE_GMII; ++ ++ plat->dma_cfg->pbl = 32; ++ plat->dma_cfg->pblx8 = true; ++ /* TODO: AXI */ + } + + static int quark_default_data(struct plat_stmmacenet_data *plat, +@@ -96,6 +120,9 @@ static int quark_default_data(struct pla + struct pci_dev *pdev = info->pdev; + int ret; + ++ /* Set common default data first */ ++ common_default_data(plat); ++ + /* + * Refuse to load the driver and register net device if MAC controller + * does not connect to any PHY interface. +@@ -107,27 +134,12 @@ static int quark_default_data(struct pla + plat->bus_id = PCI_DEVID(pdev->bus->number, pdev->devfn); + plat->phy_addr = ret; + plat->interface = PHY_INTERFACE_MODE_RMII; +- plat->clk_csr = 2; +- plat->has_gmac = 1; +- plat->force_sf_dma_mode = 1; +- +- plat->mdio_bus_data->phy_reset = NULL; +- plat->mdio_bus_data->phy_mask = 0; + + plat->dma_cfg->pbl = 16; + plat->dma_cfg->pblx8 = true; + plat->dma_cfg->fixed_burst = 1; + /* AXI (TODO) */ + +- /* Set default value for multicast hash bins */ +- plat->multicast_filter_bins = HASH_TABLE_SIZE; +- +- /* Set default value for unicast filter entries */ +- plat->unicast_filter_entries = 1; +- +- /* Set the maxmtu to a default of JUMBO_LEN */ +- plat->maxmtu = JUMBO_LEN; +- + return 0; + } + +@@ -142,6 +154,24 @@ static struct stmmac_pci_dmi_data quark_ + .func = 6, + .phy_addr = 1, + }, ++ { ++ .name = "SIMATIC IOT2000", ++ .asset_tag = "6ES7647-0AA00-0YA2", ++ .func = 6, ++ .phy_addr = 1, ++ }, ++ { ++ .name = "SIMATIC IOT2000", ++ .asset_tag = "6ES7647-0AA00-1YA2", ++ .func = 6, ++ .phy_addr = 1, ++ }, ++ { ++ .name = "SIMATIC IOT2000", ++ .asset_tag = "6ES7647-0AA00-1YA2", ++ .func = 7, ++ .phy_addr = 1, ++ }, + {} + }; + +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c +@@ -108,7 +108,7 @@ static struct stmmac_axi *stmmac_axi_set + if (!np) + return NULL; + +- axi = kzalloc(sizeof(*axi), GFP_KERNEL); ++ axi = devm_kzalloc(&pdev->dev, sizeof(*axi), GFP_KERNEL); + if (!axi) { + of_node_put(np); + return ERR_PTR(-ENOMEM); +@@ -132,6 +132,155 @@ static struct stmmac_axi *stmmac_axi_set + } + + /** ++ * stmmac_mtl_setup - parse DT parameters for multiple queues configuration ++ * @pdev: platform device ++ */ ++static void stmmac_mtl_setup(struct platform_device *pdev, ++ struct plat_stmmacenet_data *plat) ++{ ++ struct device_node *q_node; ++ struct device_node *rx_node; ++ struct device_node *tx_node; ++ u8 queue = 0; ++ ++ /* For backwards-compatibility with device trees that don't have any ++ * snps,mtl-rx-config or snps,mtl-tx-config properties, we fall back ++ * to one RX and TX queues each. ++ */ ++ plat->rx_queues_to_use = 1; ++ plat->tx_queues_to_use = 1; ++ ++ rx_node = of_parse_phandle(pdev->dev.of_node, "snps,mtl-rx-config", 0); ++ if (!rx_node) ++ return; ++ ++ tx_node = of_parse_phandle(pdev->dev.of_node, "snps,mtl-tx-config", 0); ++ if (!tx_node) { ++ of_node_put(rx_node); ++ return; ++ } ++ ++ /* Processing RX queues common config */ ++ if (of_property_read_u8(rx_node, "snps,rx-queues-to-use", ++ &plat->rx_queues_to_use)) ++ plat->rx_queues_to_use = 1; ++ ++ if (of_property_read_bool(rx_node, "snps,rx-sched-sp")) ++ plat->rx_sched_algorithm = MTL_RX_ALGORITHM_SP; ++ else if (of_property_read_bool(rx_node, "snps,rx-sched-wsp")) ++ plat->rx_sched_algorithm = MTL_RX_ALGORITHM_WSP; ++ else ++ plat->rx_sched_algorithm = MTL_RX_ALGORITHM_SP; ++ ++ /* Processing individual RX queue config */ ++ for_each_child_of_node(rx_node, q_node) { ++ if (queue >= plat->rx_queues_to_use) ++ break; ++ ++ if (of_property_read_bool(q_node, "snps,dcb-algorithm")) ++ plat->rx_queues_cfg[queue].mode_to_use = MTL_QUEUE_DCB; ++ else if (of_property_read_bool(q_node, "snps,avb-algorithm")) ++ plat->rx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB; ++ else ++ plat->rx_queues_cfg[queue].mode_to_use = MTL_QUEUE_DCB; ++ ++ if (of_property_read_u8(q_node, "snps,map-to-dma-channel", ++ &plat->rx_queues_cfg[queue].chan)) ++ plat->rx_queues_cfg[queue].chan = queue; ++ /* TODO: Dynamic mapping to be included in the future */ ++ ++ if (of_property_read_u32(q_node, "snps,priority", ++ &plat->rx_queues_cfg[queue].prio)) { ++ plat->rx_queues_cfg[queue].prio = 0; ++ plat->rx_queues_cfg[queue].use_prio = false; ++ } else { ++ plat->rx_queues_cfg[queue].use_prio = true; ++ } ++ ++ /* RX queue specific packet type routing */ ++ if (of_property_read_bool(q_node, "snps,route-avcp")) ++ plat->rx_queues_cfg[queue].pkt_route = PACKET_AVCPQ; ++ else if (of_property_read_bool(q_node, "snps,route-ptp")) ++ plat->rx_queues_cfg[queue].pkt_route = PACKET_PTPQ; ++ else if (of_property_read_bool(q_node, "snps,route-dcbcp")) ++ plat->rx_queues_cfg[queue].pkt_route = PACKET_DCBCPQ; ++ else if (of_property_read_bool(q_node, "snps,route-up")) ++ plat->rx_queues_cfg[queue].pkt_route = PACKET_UPQ; ++ else if (of_property_read_bool(q_node, "snps,route-multi-broad")) ++ plat->rx_queues_cfg[queue].pkt_route = PACKET_MCBCQ; ++ else ++ plat->rx_queues_cfg[queue].pkt_route = 0x0; ++ ++ queue++; ++ } ++ ++ /* Processing TX queues common config */ ++ if (of_property_read_u8(tx_node, "snps,tx-queues-to-use", ++ &plat->tx_queues_to_use)) ++ plat->tx_queues_to_use = 1; ++ ++ if (of_property_read_bool(tx_node, "snps,tx-sched-wrr")) ++ plat->tx_sched_algorithm = MTL_TX_ALGORITHM_WRR; ++ else if (of_property_read_bool(tx_node, "snps,tx-sched-wfq")) ++ plat->tx_sched_algorithm = MTL_TX_ALGORITHM_WFQ; ++ else if (of_property_read_bool(tx_node, "snps,tx-sched-dwrr")) ++ plat->tx_sched_algorithm = MTL_TX_ALGORITHM_DWRR; ++ else if (of_property_read_bool(tx_node, "snps,tx-sched-sp")) ++ plat->tx_sched_algorithm = MTL_TX_ALGORITHM_SP; ++ else ++ plat->tx_sched_algorithm = MTL_TX_ALGORITHM_SP; ++ ++ queue = 0; ++ ++ /* Processing individual TX queue config */ ++ for_each_child_of_node(tx_node, q_node) { ++ if (queue >= plat->tx_queues_to_use) ++ break; ++ ++ if (of_property_read_u8(q_node, "snps,weight", ++ &plat->tx_queues_cfg[queue].weight)) ++ plat->tx_queues_cfg[queue].weight = 0x10 + queue; ++ ++ if (of_property_read_bool(q_node, "snps,dcb-algorithm")) { ++ plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_DCB; ++ } else if (of_property_read_bool(q_node, ++ "snps,avb-algorithm")) { ++ plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB; ++ ++ /* Credit Base Shaper parameters used by AVB */ ++ if (of_property_read_u32(q_node, "snps,send_slope", ++ &plat->tx_queues_cfg[queue].send_slope)) ++ plat->tx_queues_cfg[queue].send_slope = 0x0; ++ if (of_property_read_u32(q_node, "snps,idle_slope", ++ &plat->tx_queues_cfg[queue].idle_slope)) ++ plat->tx_queues_cfg[queue].idle_slope = 0x0; ++ if (of_property_read_u32(q_node, "snps,high_credit", ++ &plat->tx_queues_cfg[queue].high_credit)) ++ plat->tx_queues_cfg[queue].high_credit = 0x0; ++ if (of_property_read_u32(q_node, "snps,low_credit", ++ &plat->tx_queues_cfg[queue].low_credit)) ++ plat->tx_queues_cfg[queue].low_credit = 0x0; ++ } else { ++ plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_DCB; ++ } ++ ++ if (of_property_read_u32(q_node, "snps,priority", ++ &plat->tx_queues_cfg[queue].prio)) { ++ plat->tx_queues_cfg[queue].prio = 0; ++ plat->tx_queues_cfg[queue].use_prio = false; ++ } else { ++ plat->tx_queues_cfg[queue].use_prio = true; ++ } ++ ++ queue++; ++ } ++ ++ of_node_put(rx_node); ++ of_node_put(tx_node); ++ of_node_put(q_node); ++} ++ ++/** + * stmmac_dt_phy - parse device-tree driver parameters to allocate PHY resources + * @plat: driver data platform structure + * @np: device tree node +@@ -340,6 +489,8 @@ stmmac_probe_config_dt(struct platform_d + + plat->axi = stmmac_axi_setup(pdev); + ++ stmmac_mtl_setup(pdev, plat); ++ + /* clock setup */ + plat->stmmac_clk = devm_clk_get(&pdev->dev, + STMMAC_RESOURCE_NAME); +@@ -359,13 +510,12 @@ stmmac_probe_config_dt(struct platform_d + clk_prepare_enable(plat->pclk); + + /* Fall-back to main clock in case of no PTP ref is passed */ +- plat->clk_ptp_ref = devm_clk_get(&pdev->dev, "clk_ptp_ref"); ++ plat->clk_ptp_ref = devm_clk_get(&pdev->dev, "ptp_ref"); + if (IS_ERR(plat->clk_ptp_ref)) { + plat->clk_ptp_rate = clk_get_rate(plat->stmmac_clk); + plat->clk_ptp_ref = NULL; + dev_warn(&pdev->dev, "PTP uses main clock\n"); + } else { +- clk_prepare_enable(plat->clk_ptp_ref); + plat->clk_ptp_rate = clk_get_rate(plat->clk_ptp_ref); + dev_dbg(&pdev->dev, "PTP rate %d\n", plat->clk_ptp_rate); + } +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h +@@ -59,7 +59,8 @@ + /* Enable Snapshot for Messages Relevant to Master */ + #define PTP_TCR_TSMSTRENA BIT(15) + /* Select PTP packets for Taking Snapshots */ +-#define PTP_TCR_SNAPTYPSEL_1 GENMASK(17, 16) ++#define PTP_TCR_SNAPTYPSEL_1 BIT(16) ++#define PTP_GMAC4_TCR_SNAPTYPSEL_1 GENMASK(17, 16) + /* Enable MAC address for PTP Frame Filtering */ + #define PTP_TCR_TSENMACADDR BIT(18) + +--- a/include/linux/stmmac.h ++++ b/include/linux/stmmac.h +@@ -28,6 +28,9 @@ + + #include + ++#define MTL_MAX_RX_QUEUES 8 ++#define MTL_MAX_TX_QUEUES 8 ++ + #define STMMAC_RX_COE_NONE 0 + #define STMMAC_RX_COE_TYPE1 1 + #define STMMAC_RX_COE_TYPE2 2 +@@ -44,6 +47,18 @@ + #define STMMAC_CSR_150_250M 0x4 /* MDC = clk_scr_i/102 */ + #define STMMAC_CSR_250_300M 0x5 /* MDC = clk_scr_i/122 */ + ++/* MTL algorithms identifiers */ ++#define MTL_TX_ALGORITHM_WRR 0x0 ++#define MTL_TX_ALGORITHM_WFQ 0x1 ++#define MTL_TX_ALGORITHM_DWRR 0x2 ++#define MTL_TX_ALGORITHM_SP 0x3 ++#define MTL_RX_ALGORITHM_SP 0x4 ++#define MTL_RX_ALGORITHM_WSP 0x5 ++ ++/* RX/TX Queue Mode */ ++#define MTL_QUEUE_AVB 0x0 ++#define MTL_QUEUE_DCB 0x1 ++ + /* The MDC clock could be set higher than the IEEE 802.3 + * specified frequency limit 0f 2.5 MHz, by programming a clock divider + * of value different than the above defined values. The resultant MDIO +@@ -109,6 +124,26 @@ struct stmmac_axi { + bool axi_rb; + }; + ++struct stmmac_rxq_cfg { ++ u8 mode_to_use; ++ u8 chan; ++ u8 pkt_route; ++ bool use_prio; ++ u32 prio; ++}; ++ ++struct stmmac_txq_cfg { ++ u8 weight; ++ u8 mode_to_use; ++ /* Credit Base Shaper parameters */ ++ u32 send_slope; ++ u32 idle_slope; ++ u32 high_credit; ++ u32 low_credit; ++ bool use_prio; ++ u32 prio; ++}; ++ + struct plat_stmmacenet_data { + int bus_id; + int phy_addr; +@@ -133,6 +168,12 @@ struct plat_stmmacenet_data { + int unicast_filter_entries; + int tx_fifo_size; + int rx_fifo_size; ++ u8 rx_queues_to_use; ++ u8 tx_queues_to_use; ++ u8 rx_sched_algorithm; ++ u8 tx_sched_algorithm; ++ struct stmmac_rxq_cfg rx_queues_cfg[MTL_MAX_RX_QUEUES]; ++ struct stmmac_txq_cfg tx_queues_cfg[MTL_MAX_TX_QUEUES]; + void (*fix_mac_speed)(void *priv, unsigned int speed); + int (*init)(struct platform_device *pdev, void *priv); + void (*exit)(struct platform_device *pdev, void *priv); diff --git a/target/linux/sunxi/patches-4.9/0053-stmmac-form-4-13.patch b/target/linux/sunxi/patches-4.9/0053-stmmac-form-4-13.patch new file mode 100644 index 0000000000..f829b79dc3 --- /dev/null +++ b/target/linux/sunxi/patches-4.9/0053-stmmac-form-4-13.patch @@ -0,0 +1,1924 @@ +--- a/drivers/net/ethernet/stmicro/stmmac/Kconfig ++++ b/drivers/net/ethernet/stmicro/stmmac/Kconfig +@@ -145,6 +145,17 @@ config DWMAC_SUNXI + This selects Allwinner SoC glue layer support for the + stmmac device driver. This driver is used for A20/A31 + GMAC ethernet controller. ++ ++config DWMAC_SUN8I ++ tristate "Allwinner sun8i GMAC support" ++ default ARCH_SUNXI ++ depends on OF && (ARCH_SUNXI || COMPILE_TEST) ++ ---help--- ++ Support for Allwinner H3 A83T A64 EMAC ethernet controllers. ++ ++ This selects Allwinner SoC glue layer support for the ++ stmmac device driver. This driver is used for H3/A83T/A64 ++ EMAC ethernet controller. + endif + + config STMMAC_PCI +--- a/drivers/net/ethernet/stmicro/stmmac/Makefile ++++ b/drivers/net/ethernet/stmicro/stmmac/Makefile +@@ -16,6 +16,7 @@ obj-$(CONFIG_DWMAC_SOCFPGA) += dwmac-alt + obj-$(CONFIG_DWMAC_STI) += dwmac-sti.o + obj-$(CONFIG_DWMAC_STM32) += dwmac-stm32.o + obj-$(CONFIG_DWMAC_SUNXI) += dwmac-sunxi.o ++obj-$(CONFIG_DWMAC_SUN8I) += dwmac-sun8i.o + obj-$(CONFIG_DWMAC_DWC_QOS_ETH) += dwmac-dwc-qos-eth.o + obj-$(CONFIG_DWMAC_GENERIC) += dwmac-generic.o + stmmac-platform-objs:= stmmac_platform.o +--- a/drivers/net/ethernet/stmicro/stmmac/common.h ++++ b/drivers/net/ethernet/stmicro/stmmac/common.h +@@ -549,9 +549,11 @@ extern const struct stmmac_hwtimestamp s + extern const struct stmmac_mode_ops dwmac4_ring_mode_ops; + + struct mac_link { +- int port; +- int duplex; +- int speed; ++ u32 speed_mask; ++ u32 speed10; ++ u32 speed100; ++ u32 speed1000; ++ u32 duplex; + }; + + struct mii_regs { +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c +@@ -269,7 +269,10 @@ static int socfpga_dwmac_set_phy_mode(st + ctrl &= ~(SYSMGR_EMACGRP_CTRL_PHYSEL_MASK << reg_shift); + ctrl |= val << reg_shift; + +- if (dwmac->f2h_ptp_ref_clk) { ++ if (dwmac->f2h_ptp_ref_clk || ++ phymode == PHY_INTERFACE_MODE_MII || ++ phymode == PHY_INTERFACE_MODE_GMII || ++ phymode == PHY_INTERFACE_MODE_SGMII) { + ctrl |= SYSMGR_EMACGRP_CTRL_PTP_REF_CLK_MASK << (reg_shift / 2); + regmap_read(sys_mgr_base_addr, SYSMGR_FPGAGRP_MODULE_REG, + &module); +--- /dev/null ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-sun8i.c +@@ -0,0 +1,1007 @@ ++/* ++ * dwmac-sun8i.c - Allwinner sun8i DWMAC specific glue layer ++ * ++ * Copyright (C) 2017 Corentin Labbe ++ * ++ * This program is free software; you can redistribute it and/or modify ++ * it under the terms of the GNU General Public License as published by ++ * the Free Software Foundation; either version 2 of the License, or ++ * (at your option) any later version. ++ * ++ * This program is distributed in the hope that it will be useful, ++ * but WITHOUT ANY WARRANTY; without even the implied warranty of ++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ++ * GNU General Public License for more details. ++ */ ++ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#include "stmmac.h" ++#include "stmmac_platform.h" ++ ++/* General notes on dwmac-sun8i: ++ * Locking: no locking is necessary in this file because all necessary locking ++ * is done in the "stmmac files" ++ */ ++ ++/* struct emac_variant - Descrive dwmac-sun8i hardware variant ++ * @default_syscon_value: The default value of the EMAC register in syscon ++ * This value is used for disabling properly EMAC ++ * and used as a good starting value in case of the ++ * boot process(uboot) leave some stuff. ++ * @internal_phy: Does the MAC embed an internal PHY ++ * @support_mii: Does the MAC handle MII ++ * @support_rmii: Does the MAC handle RMII ++ * @support_rgmii: Does the MAC handle RGMII ++ */ ++struct emac_variant { ++ u32 default_syscon_value; ++ int internal_phy; ++ bool support_mii; ++ bool support_rmii; ++ bool support_rgmii; ++}; ++ ++/* struct sunxi_priv_data - hold all sunxi private data ++ * @tx_clk: reference to MAC TX clock ++ * @ephy_clk: reference to the optional EPHY clock for the internal PHY ++ * @regulator: reference to the optional regulator ++ * @rst_ephy: reference to the optional EPHY reset for the internal PHY ++ * @variant: reference to the current board variant ++ * @regmap: regmap for using the syscon ++ * @use_internal_phy: Does the current PHY choice imply using the internal PHY ++ */ ++struct sunxi_priv_data { ++ struct clk *tx_clk; ++ struct clk *ephy_clk; ++ struct regulator *regulator; ++ struct reset_control *rst_ephy; ++ const struct emac_variant *variant; ++ struct regmap *regmap; ++ bool use_internal_phy; ++}; ++ ++static const struct emac_variant emac_variant_h3 = { ++ .default_syscon_value = 0x58000, ++ .internal_phy = PHY_INTERFACE_MODE_MII, ++ .support_mii = true, ++ .support_rmii = true, ++ .support_rgmii = true ++}; ++ ++static const struct emac_variant emac_variant_v3s = { ++ .default_syscon_value = 0x38000, ++ .internal_phy = PHY_INTERFACE_MODE_MII, ++ .support_mii = true ++}; ++ ++static const struct emac_variant emac_variant_a83t = { ++ .default_syscon_value = 0, ++ .internal_phy = 0, ++ .support_mii = true, ++ .support_rgmii = true ++}; ++ ++static const struct emac_variant emac_variant_a64 = { ++ .default_syscon_value = 0, ++ .internal_phy = 0, ++ .support_mii = true, ++ .support_rmii = true, ++ .support_rgmii = true ++}; ++ ++#define EMAC_BASIC_CTL0 0x00 ++#define EMAC_BASIC_CTL1 0x04 ++#define EMAC_INT_STA 0x08 ++#define EMAC_INT_EN 0x0C ++#define EMAC_TX_CTL0 0x10 ++#define EMAC_TX_CTL1 0x14 ++#define EMAC_TX_FLOW_CTL 0x1C ++#define EMAC_TX_DESC_LIST 0x20 ++#define EMAC_RX_CTL0 0x24 ++#define EMAC_RX_CTL1 0x28 ++#define EMAC_RX_DESC_LIST 0x34 ++#define EMAC_RX_FRM_FLT 0x38 ++#define EMAC_MDIO_CMD 0x48 ++#define EMAC_MDIO_DATA 0x4C ++#define EMAC_MACADDR_HI(reg) (0x50 + (reg) * 8) ++#define EMAC_MACADDR_LO(reg) (0x54 + (reg) * 8) ++#define EMAC_TX_DMA_STA 0xB0 ++#define EMAC_TX_CUR_DESC 0xB4 ++#define EMAC_TX_CUR_BUF 0xB8 ++#define EMAC_RX_DMA_STA 0xC0 ++#define EMAC_RX_CUR_DESC 0xC4 ++#define EMAC_RX_CUR_BUF 0xC8 ++ ++/* Use in EMAC_BASIC_CTL0 */ ++#define EMAC_DUPLEX_FULL BIT(0) ++#define EMAC_LOOPBACK BIT(1) ++#define EMAC_SPEED_1000 0 ++#define EMAC_SPEED_100 (0x03 << 2) ++#define EMAC_SPEED_10 (0x02 << 2) ++ ++/* Use in EMAC_BASIC_CTL1 */ ++#define EMAC_BURSTLEN_SHIFT 24 ++ ++/* Used in EMAC_RX_FRM_FLT */ ++#define EMAC_FRM_FLT_RXALL BIT(0) ++#define EMAC_FRM_FLT_CTL BIT(13) ++#define EMAC_FRM_FLT_MULTICAST BIT(16) ++ ++/* Used in RX_CTL1*/ ++#define EMAC_RX_MD BIT(1) ++#define EMAC_RX_TH_MASK GENMASK(4, 5) ++#define EMAC_RX_TH_32 0 ++#define EMAC_RX_TH_64 (0x1 << 4) ++#define EMAC_RX_TH_96 (0x2 << 4) ++#define EMAC_RX_TH_128 (0x3 << 4) ++#define EMAC_RX_DMA_EN BIT(30) ++#define EMAC_RX_DMA_START BIT(31) ++ ++/* Used in TX_CTL1*/ ++#define EMAC_TX_MD BIT(1) ++#define EMAC_TX_NEXT_FRM BIT(2) ++#define EMAC_TX_TH_MASK GENMASK(8, 10) ++#define EMAC_TX_TH_64 0 ++#define EMAC_TX_TH_128 (0x1 << 8) ++#define EMAC_TX_TH_192 (0x2 << 8) ++#define EMAC_TX_TH_256 (0x3 << 8) ++#define EMAC_TX_DMA_EN BIT(30) ++#define EMAC_TX_DMA_START BIT(31) ++ ++/* Used in RX_CTL0 */ ++#define EMAC_RX_RECEIVER_EN BIT(31) ++#define EMAC_RX_DO_CRC BIT(27) ++#define EMAC_RX_FLOW_CTL_EN BIT(16) ++ ++/* Used in TX_CTL0 */ ++#define EMAC_TX_TRANSMITTER_EN BIT(31) ++ ++/* Used in EMAC_TX_FLOW_CTL */ ++#define EMAC_TX_FLOW_CTL_EN BIT(0) ++ ++/* Used in EMAC_INT_STA */ ++#define EMAC_TX_INT BIT(0) ++#define EMAC_TX_DMA_STOP_INT BIT(1) ++#define EMAC_TX_BUF_UA_INT BIT(2) ++#define EMAC_TX_TIMEOUT_INT BIT(3) ++#define EMAC_TX_UNDERFLOW_INT BIT(4) ++#define EMAC_TX_EARLY_INT BIT(5) ++#define EMAC_RX_INT BIT(8) ++#define EMAC_RX_BUF_UA_INT BIT(9) ++#define EMAC_RX_DMA_STOP_INT BIT(10) ++#define EMAC_RX_TIMEOUT_INT BIT(11) ++#define EMAC_RX_OVERFLOW_INT BIT(12) ++#define EMAC_RX_EARLY_INT BIT(13) ++#define EMAC_RGMII_STA_INT BIT(16) ++ ++#define MAC_ADDR_TYPE_DST BIT(31) ++ ++/* H3 specific bits for EPHY */ ++#define H3_EPHY_ADDR_SHIFT 20 ++#define H3_EPHY_CLK_SEL BIT(18) /* 1: 24MHz, 0: 25MHz */ ++#define H3_EPHY_LED_POL BIT(17) /* 1: active low, 0: active high */ ++#define H3_EPHY_SHUTDOWN BIT(16) /* 1: shutdown, 0: power up */ ++#define H3_EPHY_SELECT BIT(15) /* 1: internal PHY, 0: external PHY */ ++ ++/* H3/A64 specific bits */ ++#define SYSCON_RMII_EN BIT(13) /* 1: enable RMII (overrides EPIT) */ ++ ++/* Generic system control EMAC_CLK bits */ ++#define SYSCON_ETXDC_MASK GENMASK(2, 0) ++#define SYSCON_ETXDC_SHIFT 10 ++#define SYSCON_ERXDC_MASK GENMASK(4, 0) ++#define SYSCON_ERXDC_SHIFT 5 ++/* EMAC PHY Interface Type */ ++#define SYSCON_EPIT BIT(2) /* 1: RGMII, 0: MII */ ++#define SYSCON_ETCS_MASK GENMASK(1, 0) ++#define SYSCON_ETCS_MII 0x0 ++#define SYSCON_ETCS_EXT_GMII 0x1 ++#define SYSCON_ETCS_INT_GMII 0x2 ++#define SYSCON_EMAC_REG 0x30 ++ ++/* sun8i_dwmac_dma_reset() - reset the EMAC ++ * Called from stmmac via stmmac_dma_ops->reset ++ */ ++static int sun8i_dwmac_dma_reset(void __iomem *ioaddr) ++{ ++ writel(0, ioaddr + EMAC_RX_CTL1); ++ writel(0, ioaddr + EMAC_TX_CTL1); ++ writel(0, ioaddr + EMAC_RX_FRM_FLT); ++ writel(0, ioaddr + EMAC_RX_DESC_LIST); ++ writel(0, ioaddr + EMAC_TX_DESC_LIST); ++ writel(0, ioaddr + EMAC_INT_EN); ++ writel(0x1FFFFFF, ioaddr + EMAC_INT_STA); ++ return 0; ++} ++ ++/* sun8i_dwmac_dma_init() - initialize the EMAC ++ * Called from stmmac via stmmac_dma_ops->init ++ */ ++static void sun8i_dwmac_dma_init(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, ++ u32 dma_tx, u32 dma_rx, int atds) ++{ ++ /* Write TX and RX descriptors address */ ++ writel(dma_rx, ioaddr + EMAC_RX_DESC_LIST); ++ writel(dma_tx, ioaddr + EMAC_TX_DESC_LIST); ++ ++ writel(EMAC_RX_INT | EMAC_TX_INT, ioaddr + EMAC_INT_EN); ++ writel(0x1FFFFFF, ioaddr + EMAC_INT_STA); ++} ++ ++/* sun8i_dwmac_dump_regs() - Dump EMAC address space ++ * Called from stmmac_dma_ops->dump_regs ++ * Used for ethtool ++ */ ++static void sun8i_dwmac_dump_regs(void __iomem *ioaddr, u32 *reg_space) ++{ ++ int i; ++ ++ for (i = 0; i < 0xC8; i += 4) { ++ if (i == 0x32 || i == 0x3C) ++ continue; ++ reg_space[i / 4] = readl(ioaddr + i); ++ } ++} ++ ++/* sun8i_dwmac_dump_mac_regs() - Dump EMAC address space ++ * Called from stmmac_ops->dump_regs ++ * Used for ethtool ++ */ ++static void sun8i_dwmac_dump_mac_regs(struct mac_device_info *hw, ++ u32 *reg_space) ++{ ++ int i; ++ void __iomem *ioaddr = hw->pcsr; ++ ++ for (i = 0; i < 0xC8; i += 4) { ++ if (i == 0x32 || i == 0x3C) ++ continue; ++ reg_space[i / 4] = readl(ioaddr + i); ++ } ++} ++ ++static void sun8i_dwmac_enable_dma_irq(void __iomem *ioaddr, u32 chan) ++{ ++ writel(EMAC_RX_INT | EMAC_TX_INT, ioaddr + EMAC_INT_EN); ++} ++ ++static void sun8i_dwmac_disable_dma_irq(void __iomem *ioaddr, u32 chan) ++{ ++ writel(0, ioaddr + EMAC_INT_EN); ++} ++ ++static void sun8i_dwmac_dma_start_tx(void __iomem *ioaddr, u32 chan) ++{ ++ u32 v; ++ ++ v = readl(ioaddr + EMAC_TX_CTL1); ++ v |= EMAC_TX_DMA_START; ++ v |= EMAC_TX_DMA_EN; ++ writel(v, ioaddr + EMAC_TX_CTL1); ++} ++ ++static void sun8i_dwmac_enable_dma_transmission(void __iomem *ioaddr) ++{ ++ u32 v; ++ ++ v = readl(ioaddr + EMAC_TX_CTL1); ++ v |= EMAC_TX_DMA_START; ++ v |= EMAC_TX_DMA_EN; ++ writel(v, ioaddr + EMAC_TX_CTL1); ++} ++ ++static void sun8i_dwmac_dma_stop_tx(void __iomem *ioaddr, u32 chan) ++{ ++ u32 v; ++ ++ v = readl(ioaddr + EMAC_TX_CTL1); ++ v &= ~EMAC_TX_DMA_EN; ++ writel(v, ioaddr + EMAC_TX_CTL1); ++} ++ ++static void sun8i_dwmac_dma_start_rx(void __iomem *ioaddr, u32 chan) ++{ ++ u32 v; ++ ++ v = readl(ioaddr + EMAC_RX_CTL1); ++ v |= EMAC_RX_DMA_START; ++ v |= EMAC_RX_DMA_EN; ++ writel(v, ioaddr + EMAC_RX_CTL1); ++} ++ ++static void sun8i_dwmac_dma_stop_rx(void __iomem *ioaddr, u32 chan) ++{ ++ u32 v; ++ ++ v = readl(ioaddr + EMAC_RX_CTL1); ++ v &= ~EMAC_RX_DMA_EN; ++ writel(v, ioaddr + EMAC_RX_CTL1); ++} ++ ++static int sun8i_dwmac_dma_interrupt(void __iomem *ioaddr, ++ struct stmmac_extra_stats *x, u32 chan) ++{ ++ u32 v; ++ int ret = 0; ++ ++ v = readl(ioaddr + EMAC_INT_STA); ++ ++ if (v & EMAC_TX_INT) { ++ ret |= handle_tx; ++ x->tx_normal_irq_n++; ++ } ++ ++ if (v & EMAC_TX_DMA_STOP_INT) ++ x->tx_process_stopped_irq++; ++ ++ if (v & EMAC_TX_BUF_UA_INT) ++ x->tx_process_stopped_irq++; ++ ++ if (v & EMAC_TX_TIMEOUT_INT) ++ ret |= tx_hard_error; ++ ++ if (v & EMAC_TX_UNDERFLOW_INT) { ++ ret |= tx_hard_error; ++ x->tx_undeflow_irq++; ++ } ++ ++ if (v & EMAC_TX_EARLY_INT) ++ x->tx_early_irq++; ++ ++ if (v & EMAC_RX_INT) { ++ ret |= handle_rx; ++ x->rx_normal_irq_n++; ++ } ++ ++ if (v & EMAC_RX_BUF_UA_INT) ++ x->rx_buf_unav_irq++; ++ ++ if (v & EMAC_RX_DMA_STOP_INT) ++ x->rx_process_stopped_irq++; ++ ++ if (v & EMAC_RX_TIMEOUT_INT) ++ ret |= tx_hard_error; ++ ++ if (v & EMAC_RX_OVERFLOW_INT) { ++ ret |= tx_hard_error; ++ x->rx_overflow_irq++; ++ } ++ ++ if (v & EMAC_RX_EARLY_INT) ++ x->rx_early_irq++; ++ ++ if (v & EMAC_RGMII_STA_INT) ++ x->irq_rgmii_n++; ++ ++ writel(v, ioaddr + EMAC_INT_STA); ++ ++ return ret; ++} ++ ++static void sun8i_dwmac_dma_operation_mode(void __iomem *ioaddr, int txmode, ++ int rxmode, int rxfifosz) ++{ ++ u32 v; ++ ++ v = readl(ioaddr + EMAC_TX_CTL1); ++ if (txmode == SF_DMA_MODE) { ++ v |= EMAC_TX_MD; ++ /* Undocumented bit (called TX_NEXT_FRM in BSP), the original ++ * comment is ++ * "Operating on second frame increase the performance ++ * especially when transmit store-and-forward is used." ++ */ ++ v |= EMAC_TX_NEXT_FRM; ++ } else { ++ v &= ~EMAC_TX_MD; ++ v &= ~EMAC_TX_TH_MASK; ++ if (txmode < 64) ++ v |= EMAC_TX_TH_64; ++ else if (txmode < 128) ++ v |= EMAC_TX_TH_128; ++ else if (txmode < 192) ++ v |= EMAC_TX_TH_192; ++ else if (txmode < 256) ++ v |= EMAC_TX_TH_256; ++ } ++ writel(v, ioaddr + EMAC_TX_CTL1); ++ ++ v = readl(ioaddr + EMAC_RX_CTL1); ++ if (rxmode == SF_DMA_MODE) { ++ v |= EMAC_RX_MD; ++ } else { ++ v &= ~EMAC_RX_MD; ++ v &= ~EMAC_RX_TH_MASK; ++ if (rxmode < 32) ++ v |= EMAC_RX_TH_32; ++ else if (rxmode < 64) ++ v |= EMAC_RX_TH_64; ++ else if (rxmode < 96) ++ v |= EMAC_RX_TH_96; ++ else if (rxmode < 128) ++ v |= EMAC_RX_TH_128; ++ } ++ writel(v, ioaddr + EMAC_RX_CTL1); ++} ++ ++static const struct stmmac_dma_ops sun8i_dwmac_dma_ops = { ++ .reset = sun8i_dwmac_dma_reset, ++ .init = sun8i_dwmac_dma_init, ++ .dump_regs = sun8i_dwmac_dump_regs, ++ .dma_mode = sun8i_dwmac_dma_operation_mode, ++ .enable_dma_transmission = sun8i_dwmac_enable_dma_transmission, ++ .enable_dma_irq = sun8i_dwmac_enable_dma_irq, ++ .disable_dma_irq = sun8i_dwmac_disable_dma_irq, ++ .start_tx = sun8i_dwmac_dma_start_tx, ++ .stop_tx = sun8i_dwmac_dma_stop_tx, ++ .start_rx = sun8i_dwmac_dma_start_rx, ++ .stop_rx = sun8i_dwmac_dma_stop_rx, ++ .dma_interrupt = sun8i_dwmac_dma_interrupt, ++}; ++ ++static int sun8i_dwmac_init(struct platform_device *pdev, void *priv) ++{ ++ struct sunxi_priv_data *gmac = priv; ++ int ret; ++ ++ if (gmac->regulator) { ++ ret = regulator_enable(gmac->regulator); ++ if (ret) { ++ dev_err(&pdev->dev, "Fail to enable regulator\n"); ++ return ret; ++ } ++ } ++ ++ ret = clk_prepare_enable(gmac->tx_clk); ++ if (ret) { ++ if (gmac->regulator) ++ regulator_disable(gmac->regulator); ++ dev_err(&pdev->dev, "Could not enable AHB clock\n"); ++ return ret; ++ } ++ ++ return 0; ++} ++ ++static void sun8i_dwmac_core_init(struct mac_device_info *hw, int mtu) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 v; ++ ++ v = (8 << EMAC_BURSTLEN_SHIFT); /* burst len */ ++ writel(v, ioaddr + EMAC_BASIC_CTL1); ++} ++ ++static void sun8i_dwmac_set_mac(void __iomem *ioaddr, bool enable) ++{ ++ u32 t, r; ++ ++ t = readl(ioaddr + EMAC_TX_CTL0); ++ r = readl(ioaddr + EMAC_RX_CTL0); ++ if (enable) { ++ t |= EMAC_TX_TRANSMITTER_EN; ++ r |= EMAC_RX_RECEIVER_EN; ++ } else { ++ t &= ~EMAC_TX_TRANSMITTER_EN; ++ r &= ~EMAC_RX_RECEIVER_EN; ++ } ++ writel(t, ioaddr + EMAC_TX_CTL0); ++ writel(r, ioaddr + EMAC_RX_CTL0); ++} ++ ++/* Set MAC address at slot reg_n ++ * All slot > 0 need to be enabled with MAC_ADDR_TYPE_DST ++ * If addr is NULL, clear the slot ++ */ ++static void sun8i_dwmac_set_umac_addr(struct mac_device_info *hw, ++ unsigned char *addr, ++ unsigned int reg_n) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 v; ++ ++ if (!addr) { ++ writel(0, ioaddr + EMAC_MACADDR_HI(reg_n)); ++ return; ++ } ++ ++ stmmac_set_mac_addr(ioaddr, addr, EMAC_MACADDR_HI(reg_n), ++ EMAC_MACADDR_LO(reg_n)); ++ if (reg_n > 0) { ++ v = readl(ioaddr + EMAC_MACADDR_HI(reg_n)); ++ v |= MAC_ADDR_TYPE_DST; ++ writel(v, ioaddr + EMAC_MACADDR_HI(reg_n)); ++ } ++} ++ ++static void sun8i_dwmac_get_umac_addr(struct mac_device_info *hw, ++ unsigned char *addr, ++ unsigned int reg_n) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ ++ stmmac_get_mac_addr(ioaddr, addr, EMAC_MACADDR_HI(reg_n), ++ EMAC_MACADDR_LO(reg_n)); ++} ++ ++/* caution this function must return non 0 to work */ ++static int sun8i_dwmac_rx_ipc_enable(struct mac_device_info *hw) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 v; ++ ++ v = readl(ioaddr + EMAC_RX_CTL0); ++ v |= EMAC_RX_DO_CRC; ++ writel(v, ioaddr + EMAC_RX_CTL0); ++ ++ return 1; ++} ++ ++static void sun8i_dwmac_set_filter(struct mac_device_info *hw, ++ struct net_device *dev) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 v; ++ int i = 1; ++ struct netdev_hw_addr *ha; ++ int macaddrs = netdev_uc_count(dev) + netdev_mc_count(dev) + 1; ++ ++ v = EMAC_FRM_FLT_CTL; ++ ++ if (dev->flags & IFF_PROMISC) { ++ v = EMAC_FRM_FLT_RXALL; ++ } else if (dev->flags & IFF_ALLMULTI) { ++ v |= EMAC_FRM_FLT_MULTICAST; ++ } else if (macaddrs <= hw->unicast_filter_entries) { ++ if (!netdev_mc_empty(dev)) { ++ netdev_for_each_mc_addr(ha, dev) { ++ sun8i_dwmac_set_umac_addr(hw, ha->addr, i); ++ i++; ++ } ++ } ++ if (!netdev_uc_empty(dev)) { ++ netdev_for_each_uc_addr(ha, dev) { ++ sun8i_dwmac_set_umac_addr(hw, ha->addr, i); ++ i++; ++ } ++ } ++ } else { ++ netdev_info(dev, "Too many address, switching to promiscuous\n"); ++ v = EMAC_FRM_FLT_RXALL; ++ } ++ ++ /* Disable unused address filter slots */ ++ while (i < hw->unicast_filter_entries) ++ sun8i_dwmac_set_umac_addr(hw, NULL, i++); ++ ++ writel(v, ioaddr + EMAC_RX_FRM_FLT); ++} ++ ++static void sun8i_dwmac_flow_ctrl(struct mac_device_info *hw, ++ unsigned int duplex, unsigned int fc, ++ unsigned int pause_time, u32 tx_cnt) ++{ ++ void __iomem *ioaddr = hw->pcsr; ++ u32 v; ++ ++ v = readl(ioaddr + EMAC_RX_CTL0); ++ if (fc == FLOW_AUTO) ++ v |= EMAC_RX_FLOW_CTL_EN; ++ else ++ v &= ~EMAC_RX_FLOW_CTL_EN; ++ writel(v, ioaddr + EMAC_RX_CTL0); ++ ++ v = readl(ioaddr + EMAC_TX_FLOW_CTL); ++ if (fc == FLOW_AUTO) ++ v |= EMAC_TX_FLOW_CTL_EN; ++ else ++ v &= ~EMAC_TX_FLOW_CTL_EN; ++ writel(v, ioaddr + EMAC_TX_FLOW_CTL); ++} ++ ++static int sun8i_dwmac_reset(struct stmmac_priv *priv) ++{ ++ u32 v; ++ int err; ++ ++ v = readl(priv->ioaddr + EMAC_BASIC_CTL1); ++ writel(v | 0x01, priv->ioaddr + EMAC_BASIC_CTL1); ++ ++ /* The timeout was previoulsy set to 10ms, but some board (OrangePI0) ++ * need more if no cable plugged. 100ms seems OK ++ */ ++ err = readl_poll_timeout(priv->ioaddr + EMAC_BASIC_CTL1, v, ++ !(v & 0x01), 100, 100000); ++ ++ if (err) { ++ dev_err(priv->device, "EMAC reset timeout\n"); ++ return -EFAULT; ++ } ++ return 0; ++} ++ ++static int sun8i_dwmac_set_syscon(struct stmmac_priv *priv) ++{ ++ struct sunxi_priv_data *gmac = priv->plat->bsp_priv; ++ struct device_node *node = priv->device->of_node; ++ int ret; ++ u32 reg, val; ++ ++ regmap_read(gmac->regmap, SYSCON_EMAC_REG, &val); ++ reg = gmac->variant->default_syscon_value; ++ if (reg != val) ++ dev_warn(priv->device, ++ "Current syscon value is not the default %x (expect %x)\n", ++ val, reg); ++ ++ if (gmac->variant->internal_phy) { ++ if (!gmac->use_internal_phy) { ++ /* switch to external PHY interface */ ++ reg &= ~H3_EPHY_SELECT; ++ } else { ++ reg |= H3_EPHY_SELECT; ++ reg &= ~H3_EPHY_SHUTDOWN; ++ dev_dbg(priv->device, "Select internal_phy %x\n", reg); ++ ++ if (of_property_read_bool(priv->plat->phy_node, ++ "allwinner,leds-active-low")) ++ reg |= H3_EPHY_LED_POL; ++ else ++ reg &= ~H3_EPHY_LED_POL; ++ ++ /* Force EPHY xtal frequency to 24MHz. */ ++ reg |= H3_EPHY_CLK_SEL; ++ ++ ret = of_mdio_parse_addr(priv->device, ++ priv->plat->phy_node); ++ if (ret < 0) { ++ dev_err(priv->device, "Could not parse MDIO addr\n"); ++ return ret; ++ } ++ /* of_mdio_parse_addr returns a valid (0 ~ 31) PHY ++ * address. No need to mask it again. ++ */ ++ reg |= ret << H3_EPHY_ADDR_SHIFT; ++ } ++ } ++ ++ if (!of_property_read_u32(node, "allwinner,tx-delay-ps", &val)) { ++ if (val % 100) { ++ dev_err(priv->device, "tx-delay must be a multiple of 100\n"); ++ return -EINVAL; ++ } ++ val /= 100; ++ dev_dbg(priv->device, "set tx-delay to %x\n", val); ++ if (val <= SYSCON_ETXDC_MASK) { ++ reg &= ~(SYSCON_ETXDC_MASK << SYSCON_ETXDC_SHIFT); ++ reg |= (val << SYSCON_ETXDC_SHIFT); ++ } else { ++ dev_err(priv->device, "Invalid TX clock delay: %d\n", ++ val); ++ return -EINVAL; ++ } ++ } ++ ++ if (!of_property_read_u32(node, "allwinner,rx-delay-ps", &val)) { ++ if (val % 100) { ++ dev_err(priv->device, "rx-delay must be a multiple of 100\n"); ++ return -EINVAL; ++ } ++ val /= 100; ++ dev_dbg(priv->device, "set rx-delay to %x\n", val); ++ if (val <= SYSCON_ERXDC_MASK) { ++ reg &= ~(SYSCON_ERXDC_MASK << SYSCON_ERXDC_SHIFT); ++ reg |= (val << SYSCON_ERXDC_SHIFT); ++ } else { ++ dev_err(priv->device, "Invalid RX clock delay: %d\n", ++ val); ++ return -EINVAL; ++ } ++ } ++ ++ /* Clear interface mode bits */ ++ reg &= ~(SYSCON_ETCS_MASK | SYSCON_EPIT); ++ if (gmac->variant->support_rmii) ++ reg &= ~SYSCON_RMII_EN; ++ ++ switch (priv->plat->interface) { ++ case PHY_INTERFACE_MODE_MII: ++ /* default */ ++ break; ++ case PHY_INTERFACE_MODE_RGMII: ++ reg |= SYSCON_EPIT | SYSCON_ETCS_INT_GMII; ++ break; ++ case PHY_INTERFACE_MODE_RMII: ++ reg |= SYSCON_RMII_EN | SYSCON_ETCS_EXT_GMII; ++ break; ++ default: ++ dev_err(priv->device, "Unsupported interface mode: %s", ++ phy_modes(priv->plat->interface)); ++ return -EINVAL; ++ } ++ ++ regmap_write(gmac->regmap, SYSCON_EMAC_REG, reg); ++ ++ return 0; ++} ++ ++static void sun8i_dwmac_unset_syscon(struct sunxi_priv_data *gmac) ++{ ++ u32 reg = gmac->variant->default_syscon_value; ++ ++ regmap_write(gmac->regmap, SYSCON_EMAC_REG, reg); ++} ++ ++static int sun8i_dwmac_power_internal_phy(struct stmmac_priv *priv) ++{ ++ struct sunxi_priv_data *gmac = priv->plat->bsp_priv; ++ int ret; ++ ++ if (!gmac->use_internal_phy) ++ return 0; ++ ++ ret = clk_prepare_enable(gmac->ephy_clk); ++ if (ret) { ++ dev_err(priv->device, "Cannot enable ephy\n"); ++ return ret; ++ } ++ ++ /* Make sure the EPHY is properly reseted, as U-Boot may leave ++ * it at deasserted state, and thus it may fail to reset EMAC. ++ */ ++ reset_control_assert(gmac->rst_ephy); ++ ++ ret = reset_control_deassert(gmac->rst_ephy); ++ if (ret) { ++ dev_err(priv->device, "Cannot deassert ephy\n"); ++ clk_disable_unprepare(gmac->ephy_clk); ++ return ret; ++ } ++ ++ return 0; ++} ++ ++static int sun8i_dwmac_unpower_internal_phy(struct sunxi_priv_data *gmac) ++{ ++ if (!gmac->use_internal_phy) ++ return 0; ++ ++ clk_disable_unprepare(gmac->ephy_clk); ++ reset_control_assert(gmac->rst_ephy); ++ return 0; ++} ++ ++/* sun8i_power_phy() - Activate the PHY: ++ * In case of error, no need to call sun8i_unpower_phy(), ++ * it will be called anyway by sun8i_dwmac_exit() ++ */ ++static int sun8i_power_phy(struct stmmac_priv *priv) ++{ ++ int ret; ++ ++ ret = sun8i_dwmac_power_internal_phy(priv); ++ if (ret) ++ return ret; ++ ++ ret = sun8i_dwmac_set_syscon(priv); ++ if (ret) ++ return ret; ++ ++ /* After changing syscon value, the MAC need reset or it will use ++ * the last value (and so the last PHY set. ++ */ ++ ret = sun8i_dwmac_reset(priv); ++ if (ret) ++ return ret; ++ return 0; ++} ++ ++static void sun8i_unpower_phy(struct sunxi_priv_data *gmac) ++{ ++ sun8i_dwmac_unset_syscon(gmac); ++ sun8i_dwmac_unpower_internal_phy(gmac); ++} ++ ++static void sun8i_dwmac_exit(struct platform_device *pdev, void *priv) ++{ ++ struct sunxi_priv_data *gmac = priv; ++ ++ sun8i_unpower_phy(gmac); ++ ++ clk_disable_unprepare(gmac->tx_clk); ++ ++ if (gmac->regulator) ++ regulator_disable(gmac->regulator); ++} ++ ++static const struct stmmac_ops sun8i_dwmac_ops = { ++ .core_init = sun8i_dwmac_core_init, ++ .set_mac = sun8i_dwmac_set_mac, ++ .dump_regs = sun8i_dwmac_dump_mac_regs, ++ .rx_ipc = sun8i_dwmac_rx_ipc_enable, ++ .set_filter = sun8i_dwmac_set_filter, ++ .flow_ctrl = sun8i_dwmac_flow_ctrl, ++ .set_umac_addr = sun8i_dwmac_set_umac_addr, ++ .get_umac_addr = sun8i_dwmac_get_umac_addr, ++}; ++ ++static struct mac_device_info *sun8i_dwmac_setup(void *ppriv) ++{ ++ struct mac_device_info *mac; ++ struct stmmac_priv *priv = ppriv; ++ int ret; ++ ++ mac = devm_kzalloc(priv->device, sizeof(*mac), GFP_KERNEL); ++ if (!mac) ++ return NULL; ++ ++ ret = sun8i_power_phy(priv); ++ if (ret) ++ return NULL; ++ ++ mac->pcsr = priv->ioaddr; ++ mac->mac = &sun8i_dwmac_ops; ++ mac->dma = &sun8i_dwmac_dma_ops; ++ ++ /* The loopback bit seems to be re-set when link change ++ * Simply mask it each time ++ * Speed 10/100/1000 are set in BIT(2)/BIT(3) ++ */ ++ mac->link.speed_mask = GENMASK(3, 2) | EMAC_LOOPBACK; ++ mac->link.speed10 = EMAC_SPEED_10; ++ mac->link.speed100 = EMAC_SPEED_100; ++ mac->link.speed1000 = EMAC_SPEED_1000; ++ mac->link.duplex = EMAC_DUPLEX_FULL; ++ mac->mii.addr = EMAC_MDIO_CMD; ++ mac->mii.data = EMAC_MDIO_DATA; ++ mac->mii.reg_shift = 4; ++ mac->mii.reg_mask = GENMASK(8, 4); ++ mac->mii.addr_shift = 12; ++ mac->mii.addr_mask = GENMASK(16, 12); ++ mac->mii.clk_csr_shift = 20; ++ mac->mii.clk_csr_mask = GENMASK(22, 20); ++ mac->unicast_filter_entries = 8; ++ ++ /* Synopsys Id is not available */ ++ priv->synopsys_id = 0; ++ ++ return mac; ++} ++ ++static int sun8i_dwmac_probe(struct platform_device *pdev) ++{ ++ struct plat_stmmacenet_data *plat_dat; ++ struct stmmac_resources stmmac_res; ++ struct sunxi_priv_data *gmac; ++ struct device *dev = &pdev->dev; ++ int ret; ++ ++ ret = stmmac_get_platform_resources(pdev, &stmmac_res); ++ if (ret) ++ return ret; ++ ++ plat_dat = stmmac_probe_config_dt(pdev, &stmmac_res.mac); ++ if (IS_ERR(plat_dat)) ++ return PTR_ERR(plat_dat); ++ ++ gmac = devm_kzalloc(dev, sizeof(*gmac), GFP_KERNEL); ++ if (!gmac) ++ return -ENOMEM; ++ ++ gmac->variant = of_device_get_match_data(&pdev->dev); ++ if (!gmac->variant) { ++ dev_err(&pdev->dev, "Missing dwmac-sun8i variant\n"); ++ return -EINVAL; ++ } ++ ++ gmac->tx_clk = devm_clk_get(dev, "stmmaceth"); ++ if (IS_ERR(gmac->tx_clk)) { ++ dev_err(dev, "Could not get TX clock\n"); ++ return PTR_ERR(gmac->tx_clk); ++ } ++ ++ /* Optional regulator for PHY */ ++ gmac->regulator = devm_regulator_get_optional(dev, "phy"); ++ if (IS_ERR(gmac->regulator)) { ++ if (PTR_ERR(gmac->regulator) == -EPROBE_DEFER) ++ return -EPROBE_DEFER; ++ dev_info(dev, "No regulator found\n"); ++ gmac->regulator = NULL; ++ } ++ ++ gmac->regmap = syscon_regmap_lookup_by_phandle(pdev->dev.of_node, ++ "syscon"); ++ if (IS_ERR(gmac->regmap)) { ++ ret = PTR_ERR(gmac->regmap); ++ dev_err(&pdev->dev, "Unable to map syscon: %d\n", ret); ++ return ret; ++ } ++ ++ plat_dat->interface = of_get_phy_mode(dev->of_node); ++ if (plat_dat->interface == gmac->variant->internal_phy) { ++ dev_info(&pdev->dev, "Will use internal PHY\n"); ++ gmac->use_internal_phy = true; ++ gmac->ephy_clk = of_clk_get(plat_dat->phy_node, 0); ++ if (IS_ERR(gmac->ephy_clk)) { ++ ret = PTR_ERR(gmac->ephy_clk); ++ dev_err(&pdev->dev, "Cannot get EPHY clock: %d\n", ret); ++ return -EINVAL; ++ } ++ ++ gmac->rst_ephy = of_reset_control_get(plat_dat->phy_node, NULL); ++ if (IS_ERR(gmac->rst_ephy)) { ++ ret = PTR_ERR(gmac->rst_ephy); ++ if (ret == -EPROBE_DEFER) ++ return ret; ++ dev_err(&pdev->dev, "No EPHY reset control found %d\n", ++ ret); ++ return -EINVAL; ++ } ++ } else { ++ dev_info(&pdev->dev, "Will use external PHY\n"); ++ gmac->use_internal_phy = false; ++ } ++ ++ /* platform data specifying hardware features and callbacks. ++ * hardware features were copied from Allwinner drivers. ++ */ ++ plat_dat->rx_coe = STMMAC_RX_COE_TYPE2; ++ plat_dat->tx_coe = 1; ++ plat_dat->has_sun8i = true; ++ plat_dat->bsp_priv = gmac; ++ plat_dat->init = sun8i_dwmac_init; ++ plat_dat->exit = sun8i_dwmac_exit; ++ plat_dat->setup = sun8i_dwmac_setup; ++ ++ ret = sun8i_dwmac_init(pdev, plat_dat->bsp_priv); ++ if (ret) ++ return ret; ++ ++ ret = stmmac_dvr_probe(&pdev->dev, plat_dat, &stmmac_res); ++ if (ret) ++ sun8i_dwmac_exit(pdev, plat_dat->bsp_priv); ++ ++ return ret; ++} ++ ++static const struct of_device_id sun8i_dwmac_match[] = { ++ { .compatible = "allwinner,sun8i-h3-emac", ++ .data = &emac_variant_h3 }, ++ { .compatible = "allwinner,sun8i-v3s-emac", ++ .data = &emac_variant_v3s }, ++ { .compatible = "allwinner,sun8i-a83t-emac", ++ .data = &emac_variant_a83t }, ++ { .compatible = "allwinner,sun50i-a64-emac", ++ .data = &emac_variant_a64 }, ++ { } ++}; ++MODULE_DEVICE_TABLE(of, sun8i_dwmac_match); ++ ++static struct platform_driver sun8i_dwmac_driver = { ++ .probe = sun8i_dwmac_probe, ++ .remove = stmmac_pltfr_remove, ++ .driver = { ++ .name = "dwmac-sun8i", ++ .pm = &stmmac_pltfr_pm_ops, ++ .of_match_table = sun8i_dwmac_match, ++ }, ++}; ++module_platform_driver(sun8i_dwmac_driver); ++ ++MODULE_AUTHOR("Corentin Labbe "); ++MODULE_DESCRIPTION("Allwinner sun8i DWMAC specific glue layer"); ++MODULE_LICENSE("GPL"); +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c +@@ -45,15 +45,17 @@ static void dwmac1000_core_init(struct m + if (hw->ps) { + value |= GMAC_CONTROL_TE; + +- if (hw->ps == SPEED_1000) { +- value &= ~GMAC_CONTROL_PS; +- } else { +- value |= GMAC_CONTROL_PS; +- +- if (hw->ps == SPEED_10) +- value &= ~GMAC_CONTROL_FES; +- else +- value |= GMAC_CONTROL_FES; ++ value &= ~hw->link.speed_mask; ++ switch (hw->ps) { ++ case SPEED_1000: ++ value |= hw->link.speed1000; ++ break; ++ case SPEED_100: ++ value |= hw->link.speed100; ++ break; ++ case SPEED_10: ++ value |= hw->link.speed10; ++ break; + } + } + +@@ -531,9 +533,11 @@ struct mac_device_info *dwmac1000_setup( + mac->mac = &dwmac1000_ops; + mac->dma = &dwmac1000_dma_ops; + +- mac->link.port = GMAC_CONTROL_PS; + mac->link.duplex = GMAC_CONTROL_DM; +- mac->link.speed = GMAC_CONTROL_FES; ++ mac->link.speed10 = GMAC_CONTROL_PS; ++ mac->link.speed100 = GMAC_CONTROL_PS | GMAC_CONTROL_FES; ++ mac->link.speed1000 = 0; ++ mac->link.speed_mask = GMAC_CONTROL_PS | GMAC_CONTROL_FES; + mac->mii.addr = GMAC_MII_ADDR; + mac->mii.data = GMAC_MII_DATA; + mac->mii.addr_shift = 11; +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c +@@ -205,8 +205,8 @@ static void dwmac1000_dump_dma_regs(void + { + int i; + +- for (i = 0; i < 22; i++) +- if ((i < 9) || (i > 17)) ++ for (i = 0; i < NUM_DWMAC1000_DMA_REGS; i++) ++ if ((i < 12) || (i > 17)) + reg_space[DMA_BUS_MODE / 4 + i] = + readl(ioaddr + DMA_BUS_MODE + i * 4); + } +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c +@@ -175,9 +175,11 @@ struct mac_device_info *dwmac100_setup(v + mac->mac = &dwmac100_ops; + mac->dma = &dwmac100_dma_ops; + +- mac->link.port = MAC_CONTROL_PS; + mac->link.duplex = MAC_CONTROL_F; +- mac->link.speed = 0; ++ mac->link.speed10 = 0; ++ mac->link.speed100 = 0; ++ mac->link.speed1000 = 0; ++ mac->link.speed_mask = MAC_CONTROL_PS; + mac->mii.addr = MAC_MII_ADDR; + mac->mii.data = MAC_MII_DATA; + mac->mii.addr_shift = 11; +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100_dma.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100_dma.c +@@ -70,7 +70,7 @@ static void dwmac100_dump_dma_regs(void + { + int i; + +- for (i = 0; i < 9; i++) ++ for (i = 0; i < NUM_DWMAC100_DMA_REGS; i++) + reg_space[DMA_BUS_MODE / 4 + i] = + readl(ioaddr + DMA_BUS_MODE + i * 4); + +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c +@@ -35,15 +35,17 @@ static void dwmac4_core_init(struct mac_ + if (hw->ps) { + value |= GMAC_CONFIG_TE; + +- if (hw->ps == SPEED_1000) { +- value &= ~GMAC_CONFIG_PS; +- } else { +- value |= GMAC_CONFIG_PS; +- +- if (hw->ps == SPEED_10) +- value &= ~GMAC_CONFIG_FES; +- else +- value |= GMAC_CONFIG_FES; ++ value &= hw->link.speed_mask; ++ switch (hw->ps) { ++ case SPEED_1000: ++ value |= hw->link.speed1000; ++ break; ++ case SPEED_100: ++ value |= hw->link.speed100; ++ break; ++ case SPEED_10: ++ value |= hw->link.speed10; ++ break; + } + } + +@@ -115,7 +117,7 @@ static void dwmac4_tx_queue_routing(stru + void __iomem *ioaddr = hw->pcsr; + u32 value; + +- const struct stmmac_rx_routing route_possibilities[] = { ++ static const struct stmmac_rx_routing route_possibilities[] = { + { GMAC_RXQCTRL_AVCPQ_MASK, GMAC_RXQCTRL_AVCPQ_SHIFT }, + { GMAC_RXQCTRL_PTPQ_MASK, GMAC_RXQCTRL_PTPQ_SHIFT }, + { GMAC_RXQCTRL_DCBCPQ_MASK, GMAC_RXQCTRL_DCBCPQ_SHIFT }, +@@ -747,9 +749,11 @@ struct mac_device_info *dwmac4_setup(voi + if (mac->multicast_filter_bins) + mac->mcast_bits_log2 = ilog2(mac->multicast_filter_bins); + +- mac->link.port = GMAC_CONFIG_PS; + mac->link.duplex = GMAC_CONFIG_DM; +- mac->link.speed = GMAC_CONFIG_FES; ++ mac->link.speed10 = GMAC_CONFIG_PS; ++ mac->link.speed100 = GMAC_CONFIG_FES | GMAC_CONFIG_PS; ++ mac->link.speed1000 = 0; ++ mac->link.speed_mask = GMAC_CONFIG_FES | GMAC_CONFIG_PS; + mac->mii.addr = GMAC_MDIO_ADDR; + mac->mii.data = GMAC_MDIO_DATA; + mac->mii.addr_shift = 21; +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c +@@ -71,9 +71,9 @@ static void dwmac4_dma_axi(void __iomem + writel(value, ioaddr + DMA_SYS_BUS_MODE); + } + +-void dwmac4_dma_init_rx_chan(void __iomem *ioaddr, +- struct stmmac_dma_cfg *dma_cfg, +- u32 dma_rx_phy, u32 chan) ++static void dwmac4_dma_init_rx_chan(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, ++ u32 dma_rx_phy, u32 chan) + { + u32 value; + u32 rxpbl = dma_cfg->rxpbl ?: dma_cfg->pbl; +@@ -85,9 +85,9 @@ void dwmac4_dma_init_rx_chan(void __iome + writel(dma_rx_phy, ioaddr + DMA_CHAN_RX_BASE_ADDR(chan)); + } + +-void dwmac4_dma_init_tx_chan(void __iomem *ioaddr, +- struct stmmac_dma_cfg *dma_cfg, +- u32 dma_tx_phy, u32 chan) ++static void dwmac4_dma_init_tx_chan(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, ++ u32 dma_tx_phy, u32 chan) + { + u32 value; + u32 txpbl = dma_cfg->txpbl ?: dma_cfg->pbl; +@@ -99,8 +99,8 @@ void dwmac4_dma_init_tx_chan(void __iome + writel(dma_tx_phy, ioaddr + DMA_CHAN_TX_BASE_ADDR(chan)); + } + +-void dwmac4_dma_init_channel(void __iomem *ioaddr, +- struct stmmac_dma_cfg *dma_cfg, u32 chan) ++static void dwmac4_dma_init_channel(void __iomem *ioaddr, ++ struct stmmac_dma_cfg *dma_cfg, u32 chan) + { + u32 value; + +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h +@@ -136,6 +136,9 @@ + #define DMA_STATUS_TI 0x00000001 /* Transmit Interrupt */ + #define DMA_CONTROL_FTF 0x00100000 /* Flush transmit FIFO */ + ++#define NUM_DWMAC100_DMA_REGS 9 ++#define NUM_DWMAC1000_DMA_REGS 23 ++ + void dwmac_enable_dma_transmission(void __iomem *ioaddr); + void dwmac_enable_dma_irq(void __iomem *ioaddr, u32 chan); + void dwmac_disable_dma_irq(void __iomem *ioaddr, u32 chan); +--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c ++++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c +@@ -248,6 +248,7 @@ void stmmac_set_mac_addr(void __iomem *i + data = (addr[3] << 24) | (addr[2] << 16) | (addr[1] << 8) | addr[0]; + writel(data, ioaddr + low); + } ++EXPORT_SYMBOL_GPL(stmmac_set_mac_addr); + + /* Enable disable MAC RX/TX */ + void stmmac_set_mac(void __iomem *ioaddr, bool enable) +@@ -279,4 +280,4 @@ void stmmac_get_mac_addr(void __iomem *i + addr[4] = hi_addr & 0xff; + addr[5] = (hi_addr >> 8) & 0xff; + } +- ++EXPORT_SYMBOL_GPL(stmmac_get_mac_addr); +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h +@@ -104,7 +104,7 @@ struct stmmac_priv { + /* TX Queue */ + struct stmmac_tx_queue tx_queue[MTL_MAX_TX_QUEUES]; + +- int oldlink; ++ bool oldlink; + int speed; + int oldduplex; + unsigned int flow_ctrl; +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c +@@ -29,10 +29,12 @@ + #include "stmmac.h" + #include "dwmac_dma.h" + +-#define REG_SPACE_SIZE 0x1054 ++#define REG_SPACE_SIZE 0x1060 + #define MAC100_ETHTOOL_NAME "st_mac100" + #define GMAC_ETHTOOL_NAME "st_gmac" + ++#define ETHTOOL_DMA_OFFSET 55 ++ + struct stmmac_stats { + char stat_string[ETH_GSTRING_LEN]; + int sizeof_stat; +@@ -273,7 +275,6 @@ static int stmmac_ethtool_get_link_ksett + { + struct stmmac_priv *priv = netdev_priv(dev); + struct phy_device *phy = dev->phydev; +- int rc; + + if (priv->hw->pcs & STMMAC_PCS_RGMII || + priv->hw->pcs & STMMAC_PCS_SGMII) { +@@ -364,8 +365,8 @@ static int stmmac_ethtool_get_link_ksett + "link speed / duplex setting\n", dev->name); + return -EBUSY; + } +- rc = phy_ethtool_ksettings_get(phy, cmd); +- return rc; ++ phy_ethtool_ksettings_get(phy, cmd); ++ return 0; + } + + static int +@@ -443,6 +444,9 @@ static void stmmac_ethtool_gregs(struct + + priv->hw->mac->dump_regs(priv->hw, reg_space); + priv->hw->dma->dump_regs(priv->ioaddr, reg_space); ++ /* Copy DMA registers to where ethtool expects them */ ++ memcpy(®_space[ETHTOOL_DMA_OFFSET], ®_space[DMA_BUS_MODE / 4], ++ NUM_DWMAC1000_DMA_REGS * 4); + } + + static void +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +@@ -235,6 +235,17 @@ static void stmmac_clk_csr_set(struct st + else if ((clk_rate >= CSR_F_250M) && (clk_rate < CSR_F_300M)) + priv->clk_csr = STMMAC_CSR_250_300M; + } ++ ++ if (priv->plat->has_sun8i) { ++ if (clk_rate > 160000000) ++ priv->clk_csr = 0x03; ++ else if (clk_rate > 80000000) ++ priv->clk_csr = 0x02; ++ else if (clk_rate > 40000000) ++ priv->clk_csr = 0x01; ++ else ++ priv->clk_csr = 0; ++ } + } + + static void print_pkt(unsigned char *buf, int len) +@@ -783,7 +794,7 @@ static void stmmac_adjust_link(struct ne + struct stmmac_priv *priv = netdev_priv(dev); + struct phy_device *phydev = dev->phydev; + unsigned long flags; +- int new_state = 0; ++ bool new_state = false; + + if (!phydev) + return; +@@ -796,8 +807,8 @@ static void stmmac_adjust_link(struct ne + /* Now we make sure that we can be in full duplex mode. + * If not, we operate in half-duplex mode. */ + if (phydev->duplex != priv->oldduplex) { +- new_state = 1; +- if (!(phydev->duplex)) ++ new_state = true; ++ if (!phydev->duplex) + ctrl &= ~priv->hw->link.duplex; + else + ctrl |= priv->hw->link.duplex; +@@ -808,30 +819,17 @@ static void stmmac_adjust_link(struct ne + stmmac_mac_flow_ctrl(priv, phydev->duplex); + + if (phydev->speed != priv->speed) { +- new_state = 1; ++ new_state = true; ++ ctrl &= ~priv->hw->link.speed_mask; + switch (phydev->speed) { +- case 1000: +- if (priv->plat->has_gmac || +- priv->plat->has_gmac4) +- ctrl &= ~priv->hw->link.port; ++ case SPEED_1000: ++ ctrl |= priv->hw->link.speed1000; + break; +- case 100: +- if (priv->plat->has_gmac || +- priv->plat->has_gmac4) { +- ctrl |= priv->hw->link.port; +- ctrl |= priv->hw->link.speed; +- } else { +- ctrl &= ~priv->hw->link.port; +- } ++ case SPEED_100: ++ ctrl |= priv->hw->link.speed100; + break; +- case 10: +- if (priv->plat->has_gmac || +- priv->plat->has_gmac4) { +- ctrl |= priv->hw->link.port; +- ctrl &= ~(priv->hw->link.speed); +- } else { +- ctrl &= ~priv->hw->link.port; +- } ++ case SPEED_10: ++ ctrl |= priv->hw->link.speed10; + break; + default: + netif_warn(priv, link, priv->dev, +@@ -847,12 +845,12 @@ static void stmmac_adjust_link(struct ne + writel(ctrl, priv->ioaddr + MAC_CTRL_REG); + + if (!priv->oldlink) { +- new_state = 1; +- priv->oldlink = 1; ++ new_state = true; ++ priv->oldlink = true; + } + } else if (priv->oldlink) { +- new_state = 1; +- priv->oldlink = 0; ++ new_state = true; ++ priv->oldlink = false; + priv->speed = SPEED_UNKNOWN; + priv->oldduplex = DUPLEX_UNKNOWN; + } +@@ -915,7 +913,7 @@ static int stmmac_init_phy(struct net_de + char bus_id[MII_BUS_ID_SIZE]; + int interface = priv->plat->interface; + int max_speed = priv->plat->max_speed; +- priv->oldlink = 0; ++ priv->oldlink = false; + priv->speed = SPEED_UNKNOWN; + priv->oldduplex = DUPLEX_UNKNOWN; + +@@ -1450,7 +1448,7 @@ static void free_dma_rx_desc_resources(s + static void free_dma_tx_desc_resources(struct stmmac_priv *priv) + { + u32 tx_count = priv->plat->tx_queues_to_use; +- u32 queue = 0; ++ u32 queue; + + /* Free TX queue resources */ + for (queue = 0; queue < tx_count; queue++) { +@@ -1499,7 +1497,7 @@ static int alloc_dma_rx_desc_resources(s + sizeof(dma_addr_t), + GFP_KERNEL); + if (!rx_q->rx_skbuff_dma) +- return -ENOMEM; ++ goto err_dma; + + rx_q->rx_skbuff = kmalloc_array(DMA_RX_SIZE, + sizeof(struct sk_buff *), +@@ -1562,13 +1560,13 @@ static int alloc_dma_tx_desc_resources(s + sizeof(*tx_q->tx_skbuff_dma), + GFP_KERNEL); + if (!tx_q->tx_skbuff_dma) +- return -ENOMEM; ++ goto err_dma; + + tx_q->tx_skbuff = kmalloc_array(DMA_TX_SIZE, + sizeof(struct sk_buff *), + GFP_KERNEL); + if (!tx_q->tx_skbuff) +- goto err_dma_buffers; ++ goto err_dma; + + if (priv->extend_desc) { + tx_q->dma_etx = dma_zalloc_coherent(priv->device, +@@ -1578,7 +1576,7 @@ static int alloc_dma_tx_desc_resources(s + &tx_q->dma_tx_phy, + GFP_KERNEL); + if (!tx_q->dma_etx) +- goto err_dma_buffers; ++ goto err_dma; + } else { + tx_q->dma_tx = dma_zalloc_coherent(priv->device, + DMA_TX_SIZE * +@@ -1587,13 +1585,13 @@ static int alloc_dma_tx_desc_resources(s + &tx_q->dma_tx_phy, + GFP_KERNEL); + if (!tx_q->dma_tx) +- goto err_dma_buffers; ++ goto err_dma; + } + } + + return 0; + +-err_dma_buffers: ++err_dma: + free_dma_tx_desc_resources(priv); + + return ret; +@@ -2895,8 +2893,7 @@ static netdev_tx_t stmmac_tso_xmit(struc + priv->xstats.tx_set_ic_bit++; + } + +- if (!priv->hwts_tx_en) +- skb_tx_timestamp(skb); ++ skb_tx_timestamp(skb); + + if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) && + priv->hwts_tx_en)) { +@@ -2974,7 +2971,7 @@ static netdev_tx_t stmmac_xmit(struct sk + + /* Manage oversized TCP frames for GMAC4 device */ + if (skb_is_gso(skb) && priv->tso) { +- if (ip_hdr(skb)->protocol == IPPROTO_TCP) ++ if (skb_shinfo(skb)->gso_type & (SKB_GSO_TCPV4 | SKB_GSO_TCPV6)) + return stmmac_tso_xmit(skb, dev); + } + +@@ -3105,8 +3102,7 @@ static netdev_tx_t stmmac_xmit(struct sk + priv->xstats.tx_set_ic_bit++; + } + +- if (!priv->hwts_tx_en) +- skb_tx_timestamp(skb); ++ skb_tx_timestamp(skb); + + /* Ready to fill the first descriptor and set the OWN bit w/o any + * problems because all the descriptors are actually ready to be +@@ -3983,7 +3979,9 @@ static int stmmac_hw_init(struct stmmac_ + struct mac_device_info *mac; + + /* Identify the MAC HW device */ +- if (priv->plat->has_gmac) { ++ if (priv->plat->setup) { ++ mac = priv->plat->setup(priv); ++ } else if (priv->plat->has_gmac) { + priv->dev->priv_flags |= IFF_UNICAST_FLT; + mac = dwmac1000_setup(priv->ioaddr, + priv->plat->multicast_filter_bins, +@@ -4003,6 +4001,10 @@ static int stmmac_hw_init(struct stmmac_ + + priv->hw = mac; + ++ /* dwmac-sun8i only work in chain mode */ ++ if (priv->plat->has_sun8i) ++ chain_mode = 1; ++ + /* To use the chained or ring mode */ + if (priv->synopsys_id >= DWMAC_CORE_4_00) { + priv->hw->mode = &dwmac4_ring_mode_ops; +@@ -4131,8 +4133,15 @@ int stmmac_dvr_probe(struct device *devi + if ((phyaddr >= 0) && (phyaddr <= 31)) + priv->plat->phy_addr = phyaddr; + +- if (priv->plat->stmmac_rst) ++ if (priv->plat->stmmac_rst) { ++ ret = reset_control_assert(priv->plat->stmmac_rst); + reset_control_deassert(priv->plat->stmmac_rst); ++ /* Some reset controllers have only reset callback instead of ++ * assert + deassert callbacks pair. ++ */ ++ if (ret == -ENOTSUPP) ++ reset_control_reset(priv->plat->stmmac_rst); ++ } + + /* Init MAC and get the capabilities */ + ret = stmmac_hw_init(priv); +@@ -4149,7 +4158,7 @@ int stmmac_dvr_probe(struct device *devi + NETIF_F_RXCSUM; + + if ((priv->plat->tso_en) && (priv->dma_cap.tsoen)) { +- ndev->hw_features |= NETIF_F_TSO; ++ ndev->hw_features |= NETIF_F_TSO | NETIF_F_TSO6; + priv->tso = true; + dev_info(priv->device, "TSO feature enabled\n"); + } +@@ -4311,7 +4320,7 @@ int stmmac_suspend(struct device *dev) + } + spin_unlock_irqrestore(&priv->lock, flags); + +- priv->oldlink = 0; ++ priv->oldlink = false; + priv->speed = SPEED_UNKNOWN; + priv->oldduplex = DUPLEX_UNKNOWN; + return 0; +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c +@@ -204,6 +204,7 @@ int stmmac_mdio_register(struct net_devi + struct stmmac_priv *priv = netdev_priv(ndev); + struct stmmac_mdio_bus_data *mdio_bus_data = priv->plat->mdio_bus_data; + struct device_node *mdio_node = priv->plat->mdio_node; ++ struct device *dev = ndev->dev.parent; + int addr, found; + + if (!mdio_bus_data) +@@ -237,7 +238,7 @@ int stmmac_mdio_register(struct net_devi + else + err = mdiobus_register(new_bus); + if (err != 0) { +- netdev_err(ndev, "Cannot register the MDIO bus\n"); ++ dev_err(dev, "Cannot register the MDIO bus\n"); + goto bus_register_fail; + } + +@@ -292,7 +293,7 @@ int stmmac_mdio_register(struct net_devi + } + + if (!found && !mdio_node) { +- netdev_warn(ndev, "No PHY found\n"); ++ dev_warn(dev, "No PHY found\n"); + mdiobus_unregister(new_bus); + mdiobus_free(new_bus); + return -ENODEV; +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c +@@ -30,42 +30,39 @@ + * negative value of the address means that MAC controller is not connected + * with PHY. + */ +-struct stmmac_pci_dmi_data { +- const char *name; +- const char *asset_tag; ++struct stmmac_pci_func_data { + unsigned int func; + int phy_addr; + }; + +-struct stmmac_pci_info { +- struct pci_dev *pdev; +- int (*setup)(struct plat_stmmacenet_data *plat, +- struct stmmac_pci_info *info); +- struct stmmac_pci_dmi_data *dmi; ++struct stmmac_pci_dmi_data { ++ const struct stmmac_pci_func_data *func; ++ size_t nfuncs; + }; + +-static int stmmac_pci_find_phy_addr(struct stmmac_pci_info *info) +-{ +- const char *name = dmi_get_system_info(DMI_BOARD_NAME); +- const char *asset_tag = dmi_get_system_info(DMI_BOARD_ASSET_TAG); +- unsigned int func = PCI_FUNC(info->pdev->devfn); +- struct stmmac_pci_dmi_data *dmi; +- +- /* +- * Galileo boards with old firmware don't support DMI. We always return +- * 1 here, so at least first found MAC controller would be probed. +- */ +- if (!name) +- return 1; ++struct stmmac_pci_info { ++ int (*setup)(struct pci_dev *pdev, struct plat_stmmacenet_data *plat); ++}; + +- for (dmi = info->dmi; dmi->name && *dmi->name; dmi++) { +- if (!strcmp(dmi->name, name) && dmi->func == func) { +- /* If asset tag is provided, match on it as well. */ +- if (dmi->asset_tag && strcmp(dmi->asset_tag, asset_tag)) +- continue; +- return dmi->phy_addr; +- } +- } ++static int stmmac_pci_find_phy_addr(struct pci_dev *pdev, ++ const struct dmi_system_id *dmi_list) ++{ ++ const struct stmmac_pci_func_data *func_data; ++ const struct stmmac_pci_dmi_data *dmi_data; ++ const struct dmi_system_id *dmi_id; ++ int func = PCI_FUNC(pdev->devfn); ++ size_t n; ++ ++ dmi_id = dmi_first_match(dmi_list); ++ if (!dmi_id) ++ return -ENODEV; ++ ++ dmi_data = dmi_id->driver_data; ++ func_data = dmi_data->func; ++ ++ for (n = 0; n < dmi_data->nfuncs; n++, func_data++) ++ if (func_data->func == func) ++ return func_data->phy_addr; + + return -ENODEV; + } +@@ -100,7 +97,8 @@ static void common_default_data(struct p + plat->rx_queues_cfg[0].pkt_route = 0x0; + } + +-static void stmmac_default_data(struct plat_stmmacenet_data *plat) ++static int stmmac_default_data(struct pci_dev *pdev, ++ struct plat_stmmacenet_data *plat) + { + /* Set common default data first */ + common_default_data(plat); +@@ -112,12 +110,77 @@ static void stmmac_default_data(struct p + plat->dma_cfg->pbl = 32; + plat->dma_cfg->pblx8 = true; + /* TODO: AXI */ ++ ++ return 0; + } + +-static int quark_default_data(struct plat_stmmacenet_data *plat, +- struct stmmac_pci_info *info) ++static const struct stmmac_pci_info stmmac_pci_info = { ++ .setup = stmmac_default_data, ++}; ++ ++static const struct stmmac_pci_func_data galileo_stmmac_func_data[] = { ++ { ++ .func = 6, ++ .phy_addr = 1, ++ }, ++}; ++ ++static const struct stmmac_pci_dmi_data galileo_stmmac_dmi_data = { ++ .func = galileo_stmmac_func_data, ++ .nfuncs = ARRAY_SIZE(galileo_stmmac_func_data), ++}; ++ ++static const struct stmmac_pci_func_data iot2040_stmmac_func_data[] = { ++ { ++ .func = 6, ++ .phy_addr = 1, ++ }, ++ { ++ .func = 7, ++ .phy_addr = 1, ++ }, ++}; ++ ++static const struct stmmac_pci_dmi_data iot2040_stmmac_dmi_data = { ++ .func = iot2040_stmmac_func_data, ++ .nfuncs = ARRAY_SIZE(iot2040_stmmac_func_data), ++}; ++ ++static const struct dmi_system_id quark_pci_dmi[] = { ++ { ++ .matches = { ++ DMI_EXACT_MATCH(DMI_BOARD_NAME, "Galileo"), ++ }, ++ .driver_data = (void *)&galileo_stmmac_dmi_data, ++ }, ++ { ++ .matches = { ++ DMI_EXACT_MATCH(DMI_BOARD_NAME, "GalileoGen2"), ++ }, ++ .driver_data = (void *)&galileo_stmmac_dmi_data, ++ }, ++ { ++ .matches = { ++ DMI_EXACT_MATCH(DMI_BOARD_NAME, "SIMATIC IOT2000"), ++ DMI_EXACT_MATCH(DMI_BOARD_ASSET_TAG, ++ "6ES7647-0AA00-0YA2"), ++ }, ++ .driver_data = (void *)&galileo_stmmac_dmi_data, ++ }, ++ { ++ .matches = { ++ DMI_EXACT_MATCH(DMI_BOARD_NAME, "SIMATIC IOT2000"), ++ DMI_EXACT_MATCH(DMI_BOARD_ASSET_TAG, ++ "6ES7647-0AA00-1YA2"), ++ }, ++ .driver_data = (void *)&iot2040_stmmac_dmi_data, ++ }, ++ {} ++}; ++ ++static int quark_default_data(struct pci_dev *pdev, ++ struct plat_stmmacenet_data *plat) + { +- struct pci_dev *pdev = info->pdev; + int ret; + + /* Set common default data first */ +@@ -127,9 +190,19 @@ static int quark_default_data(struct pla + * Refuse to load the driver and register net device if MAC controller + * does not connect to any PHY interface. + */ +- ret = stmmac_pci_find_phy_addr(info); +- if (ret < 0) +- return ret; ++ ret = stmmac_pci_find_phy_addr(pdev, quark_pci_dmi); ++ if (ret < 0) { ++ /* Return error to the caller on DMI enabled boards. */ ++ if (dmi_get_system_info(DMI_BOARD_NAME)) ++ return ret; ++ ++ /* ++ * Galileo boards with old firmware don't support DMI. We always ++ * use 1 here as PHY address, so at least the first found MAC ++ * controller would be probed. ++ */ ++ ret = 1; ++ } + + plat->bus_id = PCI_DEVID(pdev->bus->number, pdev->devfn); + plat->phy_addr = ret; +@@ -143,41 +216,8 @@ static int quark_default_data(struct pla + return 0; + } + +-static struct stmmac_pci_dmi_data quark_pci_dmi_data[] = { +- { +- .name = "Galileo", +- .func = 6, +- .phy_addr = 1, +- }, +- { +- .name = "GalileoGen2", +- .func = 6, +- .phy_addr = 1, +- }, +- { +- .name = "SIMATIC IOT2000", +- .asset_tag = "6ES7647-0AA00-0YA2", +- .func = 6, +- .phy_addr = 1, +- }, +- { +- .name = "SIMATIC IOT2000", +- .asset_tag = "6ES7647-0AA00-1YA2", +- .func = 6, +- .phy_addr = 1, +- }, +- { +- .name = "SIMATIC IOT2000", +- .asset_tag = "6ES7647-0AA00-1YA2", +- .func = 7, +- .phy_addr = 1, +- }, +- {} +-}; +- +-static struct stmmac_pci_info quark_pci_info = { ++static const struct stmmac_pci_info quark_pci_info = { + .setup = quark_default_data, +- .dmi = quark_pci_dmi_data, + }; + + /** +@@ -236,15 +276,9 @@ static int stmmac_pci_probe(struct pci_d + + pci_set_master(pdev); + +- if (info) { +- info->pdev = pdev; +- if (info->setup) { +- ret = info->setup(plat, info); +- if (ret) +- return ret; +- } +- } else +- stmmac_default_data(plat); ++ ret = info->setup(pdev, plat); ++ if (ret) ++ return ret; + + pci_enable_msi(pdev); + +@@ -270,14 +304,21 @@ static void stmmac_pci_remove(struct pci + + static SIMPLE_DEV_PM_OPS(stmmac_pm_ops, stmmac_suspend, stmmac_resume); + +-#define STMMAC_VENDOR_ID 0x700 ++/* synthetic ID, no official vendor */ ++#define PCI_VENDOR_ID_STMMAC 0x700 ++ + #define STMMAC_QUARK_ID 0x0937 + #define STMMAC_DEVICE_ID 0x1108 + ++#define STMMAC_DEVICE(vendor_id, dev_id, info) { \ ++ PCI_VDEVICE(vendor_id, dev_id), \ ++ .driver_data = (kernel_ulong_t)&info \ ++ } ++ + static const struct pci_device_id stmmac_id_table[] = { +- {PCI_DEVICE(STMMAC_VENDOR_ID, STMMAC_DEVICE_ID)}, +- {PCI_DEVICE(PCI_VENDOR_ID_STMICRO, PCI_DEVICE_ID_STMICRO_MAC)}, +- {PCI_VDEVICE(INTEL, STMMAC_QUARK_ID), (kernel_ulong_t)&quark_pci_info}, ++ STMMAC_DEVICE(STMMAC, STMMAC_DEVICE_ID, stmmac_pci_info), ++ STMMAC_DEVICE(STMICRO, PCI_DEVICE_ID_STMICRO_MAC, stmmac_pci_info), ++ STMMAC_DEVICE(INTEL, STMMAC_QUARK_ID, quark_pci_info), + {} + }; + +--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c ++++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c +@@ -309,6 +309,13 @@ static int stmmac_dt_phy(struct plat_stm + struct device_node *np, struct device *dev) + { + bool mdio = true; ++ static const struct of_device_id need_mdio_ids[] = { ++ { .compatible = "snps,dwc-qos-ethernet-4.10" }, ++ { .compatible = "allwinner,sun8i-a83t-emac" }, ++ { .compatible = "allwinner,sun8i-h3-emac" }, ++ { .compatible = "allwinner,sun8i-v3s-emac" }, ++ { .compatible = "allwinner,sun50i-a64-emac" }, ++ }; + + /* If phy-handle property is passed from DT, use it as the PHY */ + plat->phy_node = of_parse_phandle(np, "phy-handle", 0); +@@ -325,8 +332,7 @@ static int stmmac_dt_phy(struct plat_stm + mdio = false; + } + +- /* exception for dwmac-dwc-qos-eth glue logic */ +- if (of_device_is_compatible(np, "snps,dwc-qos-ethernet-4.10")) { ++ if (of_match_node(need_mdio_ids, np)) { + plat->mdio_node = of_get_child_by_name(np, "mdio"); + } else { + /** +--- a/include/linux/stmmac.h ++++ b/include/linux/stmmac.h +@@ -177,6 +177,7 @@ struct plat_stmmacenet_data { + void (*fix_mac_speed)(void *priv, unsigned int speed); + int (*init)(struct platform_device *pdev, void *priv); + void (*exit)(struct platform_device *pdev, void *priv); ++ struct mac_device_info *(*setup)(void *priv); + void *bsp_priv; + struct clk *stmmac_clk; + struct clk *pclk; +@@ -185,6 +186,7 @@ struct plat_stmmacenet_data { + struct reset_control *stmmac_rst; + struct stmmac_axi *axi; + int has_gmac4; ++ bool has_sun8i; + bool tso_en; + int mac_port_sel_speed; + bool en_tx_lpi_clockgating;