diff options
Diffstat (limited to 'target/linux/sunxi/patches-4.9/0052-stmmac-form-4-12.patch')
| -rw-r--r-- | target/linux/sunxi/patches-4.9/0052-stmmac-form-4-12.patch | 5952 |
1 files changed, 5952 insertions, 0 deletions
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 <linux/clk.h> + #include <linux/clk-provider.h> + #include <linux/device.h> ++#include <linux/gpio/consumer.h> + #include <linux/ethtool.h> + #include <linux/io.h> ++#include <linux/iopoll.h> + #include <linux/ioport.h> + #include <linux/module.h> ++#include <linux/of_device.h> + #include <linux/of_net.h> + #include <linux/mfd/syscon.h> + #include <linux/platform_device.h> ++#include <linux/reset.h> + #include <linux/stmmac.h> + + #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 <linux/platform_device.h> + ++#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); |