diff --git a/Documentation/devicetree/bindings/net/toshiba,tc956x-dwmac.yaml b/Documentation/devicetree/bindings/net/toshiba,tc956x-dwmac.yaml
index 37dca8c78021e..610bbf03efc9a 100644
--- a/Documentation/devicetree/bindings/net/toshiba,tc956x-dwmac.yaml
+++ b/Documentation/devicetree/bindings/net/toshiba,tc956x-dwmac.yaml
@@ -35,6 +35,13 @@ properties:
 
   gpio-controller: true
 
+  tc956x,aspm-quirk:
+    type: boolean
+    description:
+      Disable ASPM L1 and its substates on this PCI function's endpoint link.
+      Set this only on platforms where TC956x L1 entry gates shared clocks and
+      stalls a neighbouring device.
+
   # We can't allOf reference Ethernet-controller.yaml because we end up with
   # contradictory $nodename rules (`ethernet@` versus `pci@`). Happily only a
   # small number of the properties are useful on TC956x so we can just reference
diff --git a/drivers/misc/tc956x_pci.c b/drivers/misc/tc956x_pci.c
index 741a0ae0d3afb..be1756f6ce780 100644
--- a/drivers/misc/tc956x_pci.c
+++ b/drivers/misc/tc956x_pci.c
@@ -46,6 +46,7 @@
 #include <linux/compiler_types.h>
 #include <linux/device.h>
 #include <linux/dev_printk.h>
+#include <linux/idr.h>
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/module.h>
@@ -71,6 +72,11 @@
 #define NCID_OFFSET			0x0000
 #define NCID_REV_ID_MASK		GENMASK(7, 0)
 
+#define NMODESTS_OFFSET			0x0004
+#define NMODESTS_MODE2			BIT(10)	/* 0 = USP x4, 1 = USP x2 */
+#define NBUSCTRL_OFFSET			0x1014
+#define NBUSCTRL_FREQ			BIT(16)	/* 0 = 250 MHz, 1 = 125 MHz */
+
 /* Reset and clock register offsets.  MAC resets and clocks are controlled
  * by bits in register 0 for MAC0, register 1 for MAC1.  Other non-MAC
  * resets and clocks (whose IDs are defined here) are controlled by bits
@@ -141,6 +147,7 @@ enum clock_id {
  */
 #define EMAC_CTL_OFFSET(_mac_id)	((_mac_id) ? 0x1074 : 0x1070)
 #define MSIGEN_OFFSET(_mac_id)		((_mac_id) ? 0xf100 : 0xf000)
+#define TC956X_MSI_VECTORS		32
 #define DWMAC_OFFSET(_mac_id)		((_mac_id) ? 0x48000 : 0x40000)
 
 /*
@@ -149,6 +156,7 @@ enum clock_id {
  * @sfr:		Mapped SFR regions (BAR 4, one per PCI function)
  * @bridge_config:	Regmap used for bridge configuration (BAR 0)
  * @reset_clock_regmap:	Regmap used for resets and clocks
+ * @instance_id:	TC956X instance ID used for auxiliary device IDs
  * @rev_id:		Chip revision ID (for quirks)
  */
 struct tc956x_chip {
@@ -156,9 +164,12 @@ struct tc956x_chip {
 	void __iomem *sfr[2];
 	void __iomem *bridge_config;
 	struct regmap *reset_clock_regmap;
+	int instance_id;
 	u8 rev_id;
 };
 
+static DEFINE_IDA(tc956x_instance_ida);
+
 static const struct regmap_config gpio_regmap_config = {
 	.name		= "tc956x-gpio",
 	.reg_bits	= 32,
@@ -235,26 +246,36 @@ static void adev_remove(void *data)
 	auxiliary_device_uninit(adev);
 }
 
+static void chip_instance_id_free(void *data)
+{
+	struct tc956x_chip *chip = data;
+
+	ida_free(&tc956x_instance_ida, chip->instance_id);
+}
+
+/* The of_node reference is always be dropped (success or not) */
 static int adev_device_add(struct device *dev, const char *name, u32 id,
-			   void *platform_data)
+			   struct device_node *of_node, void *platform_data)
 {
 	struct auxiliary_device *adev;
 	int ret;
 
 	adev = kzalloc_obj(*adev);
-	if (!adev)
+	if (!adev) {
+		of_node_put(of_node);
 		return -ENOMEM;
+	}
 
 	adev->id = id;
 	adev->name = name;
 	adev->dev.parent = dev;
 	adev->dev.platform_data = platform_data;
 	adev->dev.release = adev_release;
-	device_set_of_node_from_dev(&adev->dev, dev);
+	adev->dev.of_node = of_node;
 
 	ret = auxiliary_device_init(adev);
 	if (ret) {
-		of_node_put(adev->dev.of_node);
+		of_node_put(of_node);
 		kfree(adev);
 		return ret;
 	}
@@ -268,51 +289,145 @@ static int adev_device_add(struct device *dev, const char *name, u32 id,
 	return devm_add_action_or_reset(dev, adev_remove, adev);
 }
 
+static bool dev_node_has_mdio_child(struct device_node *np)
+{
+	struct device_node *mdio;
+
+	mdio = of_get_child_by_name(np, "mdio");
+	if (!mdio)
+		return false;
+
+	of_node_put(mdio);
+
+	return true;
+}
+
+static bool dev_node_is_gpio(struct device *dev, struct device_node *np)
+{
+	if (!of_property_present(np, "gpio-controller"))
+		return false;
+
+	if (!of_property_present(np, "#gpio-cells")) {
+		dev_err(dev, "gpio node contains no #gpio-cells property\n");
+		return false;
+	}
+
+	return true;
+}
+
+/* Returns a reference to the GPIO's DT node, or a null pointer */
+static struct device_node *dev_node_gpio(struct device *dev)
+{
+	struct device_node *np;
+
+	/* The GPIO sub-node is not required (platform might not need it) */
+	for_each_child_of_node(dev->of_node, np)
+		if (!strcmp(np->name, "gpio"))
+			break;
+	if (np) {
+		if (dev_node_is_gpio(dev, np))
+			return np;
+
+		of_node_put(np);
+
+		return NULL;
+	}
+
+	/*
+	 * The original TC956x binding placed the GPIO controller properties on
+	 * PCI function 0 itself. Keep accepting that form so existing DTs do not
+	 * need to grow a gpio sub-node.
+	 */
+	if (dev_node_is_gpio(dev, dev->of_node))
+		return of_node_get(dev->of_node);
+
+	return NULL;
+}
+
+/* Returns a reference to the XGMAC's DT node, or a null pointer */
+static struct device_node *dev_node_xgmac(struct device *dev)
+{
+	struct device_node *np;
+
+	for_each_child_of_node(dev->of_node, np)
+		if (!strcmp(np->name, "ethernet"))
+			return np;
+
+	/*
+	 * The original TC956x binding placed Ethernet controller properties on
+	 * the PCI function node. Treat that node as the XGMAC node when it has
+	 * the old shape.
+	 */
+	if (of_property_present(dev->of_node, "phy-mode") ||
+	    of_property_present(dev->of_node, "phy-connection-type") ||
+	    of_property_present(dev->of_node, "phy-handle") ||
+	    dev_node_has_mdio_child(dev->of_node))
+		return of_node_get(dev->of_node);
+
+	return NULL;
+}
+
 /* The embedded GPIO controller has an auxiliary device driver */
 static int chip_gpio_adev_add(struct tc956x_chip *chip)
 {
 	struct device *dev = chip->dev;
+	struct device_node *np;
 	struct regmap *regmap;
 
-	/* If needed, PCIe function 0 implements the GPIO controller. */
-	if (!device_property_present(dev, "gpio-controller"))
+	np = dev_node_gpio(dev);
+	if (!np)
 		return 0;
 
 	regmap = devm_regmap_init_mmio(dev, chip->sfr[0], &gpio_regmap_config);
-	if (IS_ERR(regmap))
+	if (IS_ERR(regmap)) {
+		of_node_put(np);
 		return PTR_ERR(regmap);
+	}
 
-	return adev_device_add(dev, GPIO_DEVICE_NAME, 0, regmap);
+	return adev_device_add(dev, GPIO_DEVICE_NAME, chip->instance_id, np,
+			       regmap);
 }
 
 /* The two embedded XGMAC controllers have an auxiliary device driver */
 static int function_xgmac_adev_add(struct pci_dev *pdev,
 				   struct tc956x_chip *chip,
-				   unsigned int msigen_irq)
+				   unsigned int msigen_irq,
+				   unsigned int msigen_nvec)
 {
 	u8 mac_id = PCI_FUNC(pdev->devfn);
 	struct device *dev = &pdev->dev;
 	struct tc956x_dwmac_data *data;
+	struct device_node *np;
 	void __iomem *sfr;
 	int ret;
 
 	if (mac_id > 1)
 		return -EINVAL;
+
+	np = dev_node_xgmac(dev);
+	if (!np)
+		return 0;
+
 	sfr = chip->sfr[mac_id];
 
 	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
-	if (!data)
+	if (!data) {
+		of_node_put(np);
 		return -ENOMEM;
+	}
 
 	data->chip = chip;
 	data->msigen = sfr + MSIGEN_OFFSET(mac_id);
 	data->msigen_irq = msigen_irq;
+	data->msigen_nvec = msigen_nvec;
 	data->emac = sfr + DWMAC_OFFSET(mac_id);
 	data->emac_ctl = sfr + EMAC_CTL_OFFSET(mac_id);
+	data->sfr = sfr;
 	data->rev_id = chip->rev_id;
 	data->mac_id = mac_id;
 
-	ret = adev_device_add(dev, TC956X_XGMAC_DEV_NAME, mac_id, data);
+	ret = adev_device_add(dev, TC956X_XGMAC_DEV_NAME,
+			      chip->instance_id * 2 + mac_id, np, data);
 	if (ret)
 		return ret;
 
@@ -476,6 +591,15 @@ static struct tc956x_chip *chip_get(struct pci_dev *pdev)
 		if (!chip)
 			return ERR_PTR(-ENOMEM);
 
+		ret = ida_alloc(&tc956x_instance_ida, GFP_KERNEL);
+		if (ret < 0)
+			return ERR_PTR(ret);
+		chip->instance_id = ret;
+
+		ret = devm_add_action_or_reset(dev, chip_instance_id_free, chip);
+		if (ret)
+			return ERR_PTR(ret);
+
 		/*
 		 * The function whose device pointer matches the chip's
 		 * device pointer manages common resources (like MSIGEN).
@@ -552,6 +676,7 @@ tc956x_function_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	struct device *dev = &pdev->dev;
 	struct tc956x_chip *chip;
 	unsigned int msigen_irq;
+	unsigned int msigen_nvec;
 	int ret;
 
 	/* Despite being a PCI device, we require devicetree */
@@ -564,22 +689,35 @@ tc956x_function_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	pci_set_master(pdev);
 
+	if (of_property_present(dev->of_node, "tc956x,aspm-quirk")) {
+		ret = pci_disable_link_state(pdev, PCIE_LINK_STATE_L1 |
+						   PCIE_LINK_STATE_L1_1 |
+						   PCIE_LINK_STATE_L1_2 |
+						   PCIE_LINK_STATE_L1_1_PCIPM |
+						   PCIE_LINK_STATE_L1_2_PCIPM);
+		if (ret)
+			dev_warn(dev, "failed to disable ASPM L1: %d\n", ret);
+	}
+
 	/* Function 1 gets -EPROBE_DEFER until function 0 sets platform data */
 	chip = chip_get(pdev);
 	if (IS_ERR(chip))
 		return dev_err_probe(dev, PTR_ERR(chip), "failed to get chip\n");
 
 	/* We called pcim_enable_device() so this will be freed automatically */
-	ret = pci_alloc_irq_vectors(pdev, 1, 1, PCI_IRQ_MSI);
+	ret = pci_alloc_irq_vectors(pdev, 1, TC956X_MSI_VECTORS, PCI_IRQ_MSI);
 	if (ret < 1)
 		return dev_err_probe(dev, ret ? : -EIO,
 				     "failed to allocate IRQ vectors\n");
+	msigen_nvec = ret;
 
 	ret = pci_irq_vector(pdev, 0);
 	if (ret < 1)
 		return dev_err_probe(dev, ret ? : -EIO, "failed to get IRQ\n");
 	msigen_irq = ret;
 
+	dev_info(dev, "%u MSI vector(s), base IRQ %u\n", msigen_nvec, msigen_irq);
+
 	ret = chip_init(chip, pdev);
 	if (ret)
 		return dev_err_probe(dev, ret, "failed to initialize chip\n");
@@ -587,7 +725,7 @@ tc956x_function_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	/* We're ready; the other function can now probe */
 	dev->platform_data = chip;
 
-	ret = function_xgmac_adev_add(pdev, chip, msigen_irq);
+	ret = function_xgmac_adev_add(pdev, chip, msigen_irq, msigen_nvec);
 	if (ret)
 		return dev_err_probe(dev, ret, "failed to add xgmap device\n");
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-tc956x.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-tc956x.c
index fbe1fc9e7bddd..72bfa21281c4c 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-tc956x.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-tc956x.c
@@ -10,7 +10,9 @@
 
 #include <linux/auxiliary_bus.h>
 #include <linux/bitops.h>
+#include <linux/delay.h>
 #include <linux/etherdevice.h>
+#include <linux/gpio/consumer.h>
 #include <linux/iopoll.h>
 #include <linux/irqdomain.h>
 #include <linux/irqchip/chained_irq.h>
@@ -31,16 +33,23 @@
 #define DRIVER_NAME			"dwmac-tc956x"
 
 #define TC956X_PTP_CLOCK_RATE		(250 * HZ_PER_MHZ)
+#define TC956X_MAC_CLOCK_RATE		(125 * HZ_PER_MHZ)
 
-#define TC956X_RX_FIFO_KB		46	/* Shared by all RX queues */
-#define TC956X_TX_FIFO_KB		46	/* Shared by all TX queues */
+#define TC956X_RX_FIFO_KB		32	/* Shared by all RX queues */
+#define TC956X_TX_FIFO_KB		8	/* Shared by all TX queues */
+#define TC956X_RX_QUEUES		1
+#define TC956X_TX_QUEUES		1
 
 /* Fields and values for the EMACTL registers */
 #define EMAC_SP_SEL_MASK		GENMASK(3, 0)
-#define SP_SEL_SGMII_2500M		4
+#define SP_SEL_2500BASEX		4
 #define SP_SEL_SGMII_1000M		5
 #define SP_SEL_SGMII_100M		6
 #define SP_SEL_SGMII_10M		7
+#define SP_SEL_USXGMII_10G		8
+#define SP_SEL_USXGMII_100M_10G		9
+#define SP_SEL_USXGMII_5G		10
+#define SP_SEL_USXGMII_2500M		13
 #define EMAC_PHY_INF_SEL_MASK		GENMASK(5, 4)
 #define PCS_CLK_PHY			1	/* Clock from PHY */
 #define EMAC_INV_SGM_SIG_DET		BIT(6)	/* 1 = polarity inverted */
@@ -49,9 +58,12 @@
 
 /* MSIGEN Registers */
 #define MSI_OUT_EN_OFFSET		0x0000
+#define MSI_MASK_SET_OFFSET		0x0008
 #define MSI_MASK_CLR_OFFSET		0x000c
-#define MSI_MASK_VALUE			BIT(0)
 #define MSI_INT_STS_OFFSET		0x0010
+#define MSI_VECT_SET_OFFSET(_src)	(0x0020 + ((_src) / 4) * 4)
+#define MSI_VECT_SET_SHIFT(_src)	(((_src) % 4) * 8)
+#define MSI_VECT_SET_MASK		GENMASK(4, 0)
 
 enum msigen_hwirq {
 	HWIRQ_LPI		= 0,
@@ -66,6 +78,11 @@ enum msigen_hwirq {
 };
 
 #define HWIRQ_COUNT			25
+#define TC956X_DMA_CHANS		8
+#define MSI_VEC_MISC			1
+#define MSI_VEC_TX(_ch)			(2 + (_ch))
+#define MSI_VEC_RX(_ch)			(2 + TC956X_DMA_CHANS + (_ch))
+#define MSI_VEC_COUNT			(2 + 2 * TC956X_DMA_CHANS)
 
 /* Offset to the XPCS memory block, relative to the EMAC address range */
 #define DWMAC_XPCS_OFFSET		0x3a00
@@ -73,6 +90,11 @@ enum msigen_hwirq {
 /* Offset to the PMATOP memory block, relative to the EMAC address range */
 #define DWMAC_PMATOP_OFFSET			0x4000
 
+/* TC956X SFR offsets used for USXGMII PMA/lane setup */
+#define TC956X_GLUE_PHY_REG_ACCESS_CTRL		0x2c030
+#define TC956X_PHY_CORE0_GL_LANE_ACCESS		0x28000
+#define TC956X_PMA_LN_PCS2PMA_PHYMODE_R2	0x2b268
+
 #define PMA_CML_GL_PM_CFG0			0x01b8
 
 /*
@@ -96,6 +118,12 @@ enum msigen_hwirq {
 #define COMM_CFG_WRITE_DATA_MASK		GENMASK(7, 0)
 #define WRITE_DATA_VALUE			0x04	/* Power-on value */
 
+struct tc956x_msi_vec {
+	struct tc956x_data *td;
+	u32 src_mask;		/* MSIGEN sources on this vector */
+	u8 vec;			/* MSI vector number */
+};
+
 /**
  * struct tc956x_data - Toshiba-specific platform data
  * @dev:		Device pointer
@@ -114,6 +142,9 @@ struct tc956x_data {
 	struct tc956x_dwmac_data *auxbus_data;
 	struct plat_stmmacenet_data *plat;
 
+	struct tc956x_msi_vec msi_vec[MSI_VEC_COUNT];
+	unsigned int msi_vec_used;
+
 	/* These three fields are used by the plat_stmmacenet_data structure */
 	struct stmmac_dma_cfg dma_cfg;
 	struct stmmac_mdio_bus_data mdio_bus_data;
@@ -131,11 +162,22 @@ struct tc956x_mac_speed {
 };
 
 static struct tc956x_mac_speed mac_speed[] = {
-	{ PHY_INTERFACE_MODE_2500BASEX,	SPEED_2500,  SP_SEL_SGMII_2500M, },
-	{ PHY_INTERFACE_MODE_SGMII,	SPEED_2500,  SP_SEL_SGMII_2500M, },
-	{ PHY_INTERFACE_MODE_SGMII,	SPEED_1000,  SP_SEL_SGMII_1000M, },
-	{ PHY_INTERFACE_MODE_SGMII,	SPEED_100,   SP_SEL_SGMII_100M, },
-	{ PHY_INTERFACE_MODE_SGMII,	SPEED_10,    SP_SEL_SGMII_10M, },
+	{ PHY_INTERFACE_MODE_USXGMII,	SPEED_10000, SP_SEL_USXGMII_10G },
+	{ PHY_INTERFACE_MODE_USXGMII,	SPEED_5000,  SP_SEL_USXGMII_5G },
+	{ PHY_INTERFACE_MODE_USXGMII,	SPEED_2500,  SP_SEL_USXGMII_2500M },
+	{ PHY_INTERFACE_MODE_USXGMII,	SPEED_1000,  SP_SEL_USXGMII_10G },
+	{ PHY_INTERFACE_MODE_USXGMII,	SPEED_100,   SP_SEL_USXGMII_100M_10G },
+	{ PHY_INTERFACE_MODE_USXGMII,	SPEED_10,    SP_SEL_USXGMII_100M_10G },
+	{ PHY_INTERFACE_MODE_2500BASEX,	SPEED_2500,  SP_SEL_2500BASEX },
+	/*
+	 * QCA808x switches its host interface to 2500BASE-X at 2.5G, but older
+	 * stmmac only passes speed to fix_mac_speed(). Legacy DTs therefore
+	 * still arrive here as SGMII, so program the 2500BASE-X selector.
+	 */
+	{ PHY_INTERFACE_MODE_SGMII,	SPEED_2500,  SP_SEL_2500BASEX },
+	{ PHY_INTERFACE_MODE_SGMII,	SPEED_1000,  SP_SEL_SGMII_1000M },
+	{ PHY_INTERFACE_MODE_SGMII,	SPEED_100,   SP_SEL_SGMII_100M },
+	{ PHY_INTERFACE_MODE_SGMII,	SPEED_10,    SP_SEL_SGMII_10M },
 };
 
 /* TC956x uses indirect addressing so this need only describe a 1KiB range */
@@ -149,33 +191,95 @@ static const struct regmap_config xpcs_regmap_config = {
 
 static void tc956x_msigen_irq_handler(struct irq_desc *desc)
 {
-	struct irq_domain *irq_domain = irq_desc_get_handler_data(desc);
+	struct tc956x_msi_vec *mv = irq_desc_get_handler_data(desc);
 	struct irq_chip *chip = irq_desc_get_chip(desc);
 	struct irq_chip_generic *gc;
 	unsigned long status;
 	unsigned int hwirq;
 
-	gc = irq_get_domain_generic_chip(irq_domain, 0);
+	gc = irq_get_domain_generic_chip(mv->td->irq_domain, 0);
 
 	chained_irq_enter(chip, desc);
 
-	status = irq_reg_readl(gc, MSI_INT_STS_OFFSET);
+	status = irq_reg_readl(gc, MSI_INT_STS_OFFSET) & mv->src_mask;
 	for_each_set_bit(hwirq, &status, HWIRQ_COUNT)
-		generic_handle_domain_irq(irq_domain, hwirq);
+		generic_handle_domain_irq(mv->td->irq_domain, hwirq);
 
 	/*
 	 * Clear the MSI flag. Most interrupts within TC956X are level-high
 	 * type. If any interrupts are still asserted then clearing this flag
 	 * will cause the (edge-triggered) MSI to be regenerated.
 	 */
-	irq_reg_writel(gc, MSI_MASK_VALUE, MSI_MASK_CLR_OFFSET);
+	irq_reg_writel(gc, BIT(mv->vec), MSI_MASK_CLR_OFFSET);
 
 	chained_irq_exit(chip, desc);
 }
 
+static void tc956x_msigen_route(struct irq_chip_generic *gc,
+				const u8 *src_vec, unsigned int nsrc)
+{
+	unsigned int src, reg;
+	u32 val;
+
+	irq_reg_writel(gc, 0, MSI_OUT_EN_OFFSET);
+
+	for (reg = 0; reg < DIV_ROUND_UP(nsrc, 4); reg++) {
+		val = 0;
+		for (src = reg * 4; src < min(nsrc, (reg + 1) * 4); src++)
+			val |= (src_vec[src] & MSI_VECT_SET_MASK) <<
+				MSI_VECT_SET_SHIFT(src);
+		irq_reg_writel(gc, val, MSI_VECT_SET_OFFSET(reg * 4));
+	}
+}
+
+static void tc956x_msigen_plan(struct tc956x_data *td, u8 *src_vec)
+{
+	unsigned int src, ch;
+
+	if (td->auxbus_data->msigen_nvec < MSI_VEC_COUNT) {
+		for (src = 0; src < HWIRQ_COUNT; src++)
+			src_vec[src] = 0;
+		td->msi_vec[0] = (struct tc956x_msi_vec){
+			.td = td, .vec = 0,
+			.src_mask = GENMASK(HWIRQ_COUNT - 1, 0),
+		};
+		td->msi_vec_used = 1;
+		return;
+	}
+
+	for (src = 0; src < HWIRQ_COUNT; src++)
+		src_vec[src] = MSI_VEC_MISC;
+	for (ch = 0; ch < TC956X_DMA_CHANS; ch++) {
+		src_vec[HWIRQ_TX0 + ch] = MSI_VEC_TX(ch);
+		src_vec[HWIRQ_RX0 + ch] = MSI_VEC_RX(ch);
+	}
+
+	td->msi_vec[0] = (struct tc956x_msi_vec){
+		.td = td, .vec = MSI_VEC_MISC,
+		.src_mask = GENMASK(HWIRQ_EVENT, HWIRQ_LPI) |
+			    GENMASK(HWIRQ_COUNT - 1, HWIRQ_XPCS),
+	};
+	td->msi_vec_used = 1;
+
+	/* Interleaved so that TX and RX of the same channel index differ */
+	for (ch = 0; ch < TC956X_DMA_CHANS; ch++) {
+		td->msi_vec[td->msi_vec_used++] = (struct tc956x_msi_vec){
+			.td = td, .vec = MSI_VEC_TX(ch),
+			.src_mask = BIT(HWIRQ_TX0 + ch),
+		};
+		td->msi_vec[td->msi_vec_used++] = (struct tc956x_msi_vec){
+			.td = td, .vec = MSI_VEC_RX(ch),
+			.src_mask = BIT(HWIRQ_RX0 + ch),
+		};
+	}
+}
+
 static int tc956x_msigen_irq_chip_init(struct irq_chip_generic *gc)
 {
 	struct tc956x_data *td = gc->domain->host_data;
+	u8 src_vec[HWIRQ_COUNT];
+	u32 vec_used = 0;
+	unsigned int i;
 
 	gc->reg_base = td->auxbus_data->msigen;
 	gc->chip_types[0].regs.mask = MSI_OUT_EN_OFFSET;
@@ -185,6 +289,15 @@ static int tc956x_msigen_irq_chip_init(struct irq_chip_generic *gc)
 	/* Disable all interrupts */
 	irq_reg_writel(gc, 0, MSI_OUT_EN_OFFSET);
 
+	tc956x_msigen_plan(td, src_vec);
+	tc956x_msigen_route(gc, src_vec, HWIRQ_COUNT);
+
+	for (i = 0; i < td->msi_vec_used; i++)
+		vec_used |= BIT(td->msi_vec[i].vec);
+
+	irq_reg_writel(gc, ~vec_used & ~BIT(0), MSI_MASK_SET_OFFSET);
+	irq_reg_writel(gc, vec_used, MSI_MASK_CLR_OFFSET);
+
 	return 0;
 }
 
@@ -196,10 +309,41 @@ static void tc956x_msigen_irq_chip_exit(struct irq_chip_generic *gc)
 static int tc956x_msigen_irq_domain_init(struct irq_domain *irq_domain)
 {
 	struct tc956x_data *td = irq_domain->host_data;
+	unsigned int i, cpu;
+	int last = -1;
+
+	if (td->msi_vec_used == 1) {
+		for_each_cpu(cpu, cpu_online_mask)
+			last = cpu;
+		irq_set_affinity_and_hint(td->auxbus_data->msigen_irq,
+					  cpumask_of(last > 0 ? last - 1 : 0));
+		irq_set_chained_handler_and_data(td->auxbus_data->msigen_irq,
+						 tc956x_msigen_irq_handler,
+						 &td->msi_vec[0]);
+		dev_info(td->dev, "%u MSI vector(s), sharing one interrupt\n",
+			 td->auxbus_data->msigen_nvec);
+		return 0;
+	}
+
+	cpu = cpumask_first(cpu_online_mask);
+	for (i = 0; i < td->msi_vec_used; i++) {
+		unsigned int irq = td->auxbus_data->msigen_irq +
+				   td->msi_vec[i].vec;
+
+		irq_set_chained_handler_and_data(irq,
+						 tc956x_msigen_irq_handler,
+						 &td->msi_vec[i]);
+
+		/* Leave the shared low-rate vector wherever it lands */
+		if (td->msi_vec[i].vec == MSI_VEC_MISC)
+			continue;
 
-	irq_set_chained_handler_and_data(td->auxbus_data->msigen_irq,
-					 tc956x_msigen_irq_handler,
-					 irq_domain);
+		irq_set_affinity_and_hint(irq, cpumask_of(cpu));
+		cpu = cpumask_next_wrap(cpu, cpu_online_mask);
+	}
+
+	dev_info(td->dev, "%u MSI vectors, one per DMA channel\n",
+		 td->auxbus_data->msigen_nvec);
 
 	return 0;
 }
@@ -207,11 +351,19 @@ static int tc956x_msigen_irq_domain_init(struct irq_domain *irq_domain)
 static void tc956x_msigen_irq_domain_exit(struct irq_domain *irq_domain)
 {
 	struct tc956x_data *td = irq_domain->host_data;
+	unsigned int i;
+
+	for (i = 0; i < td->msi_vec_used; i++) {
+		unsigned int irq = td->auxbus_data->msigen_irq;
+
+		if (td->msi_vec_used > 1)
+			irq += td->msi_vec[i].vec;
 
-	irq_set_chained_handler_and_data(td->auxbus_data->msigen_irq,
-					 NULL, NULL);
+		irq_set_chained_handler_and_data(irq, NULL, NULL);
+	}
 }
 
+
 /* We have one IRQ chip instance with 25 IRQs in its domain */
 static struct irq_domain *
 tc956x_msigen_irq_domain_instantiate(struct tc956x_data *td)
@@ -249,6 +401,7 @@ static void tc956x_pma_init(struct tc956x_data *td)
 {
 	const struct tc956x_chip *chip = td->auxbus_data->chip;
 	void __iomem *emac_ctl = td->auxbus_data->emac_ctl;
+	void __iomem *sfr = td->auxbus_data->sfr;
 	u32 id = td->auxbus_data->mac_id;
 	void __iomem *pmatop;
 	u32 val;
@@ -285,12 +438,18 @@ static void tc956x_pma_init(struct tc956x_data *td)
 
 	tc956x_reset_deassert(chip, id, MAC_RESET_PMA);
 
+	if (td->plat->phy_interface == PHY_INTERFACE_MODE_USXGMII) {
+		writel(0x0f, sfr + TC956X_GLUE_PHY_REG_ACCESS_CTRL);
+		writel(0x0f, sfr + TC956X_PHY_CORE0_GL_LANE_ACCESS);
+		writel(0x02, sfr + TC956X_PMA_LN_PCS2PMA_PHYMODE_R2);
+	}
+
 	WARN_ON(readl_poll_timeout(emac_ctl, val, val & EMAC_INIT_DONE, 50, 1000000));
 }
 
-static int tc956x_mac_speed_select(struct tc956x_data *td, int speed)
+static int tc956x_mac_speed_select(struct tc956x_data *td,
+				   phy_interface_t phy_interface, int speed)
 {
-	phy_interface_t phy_interface = td->plat->phy_interface;
 	struct net_device *netdev;
 	int i;
 
@@ -308,13 +467,14 @@ static int tc956x_mac_speed_select(struct tc956x_data *td, int speed)
 	return -EOPNOTSUPP;
 }
 
-static int tc956x_mac_configure(struct tc956x_data *td, int speed)
+static int tc956x_mac_configure(struct tc956x_data *td,
+				phy_interface_t phy_interface, int speed)
 {
 	void __iomem *emac_ctl = td->auxbus_data->emac_ctl;
 	int sp_sel;
 	u32 val;
 
-	sp_sel = tc956x_mac_speed_select(td, speed);
+	sp_sel = tc956x_mac_speed_select(td, phy_interface, speed);
 	if (sp_sel < 0)
 		return sp_sel;
 
@@ -341,10 +501,17 @@ static int tc956x_mac_enable(struct tc956x_data *td)
 	if (id)
 		tc956x_clock_enable(chip, id, MAC_CLOCK_RMII);
 
-	/* Set the speed related registers */
-	ret = tc956x_mac_configure(td, plat->max_speed);
+	if (plat->phy_interface == PHY_INTERFACE_MODE_USXGMII) {
+		tc956x_common_clock_enable(chip, COMMON_CLOCK_PLL);
+		tc956x_common_clock_enable(chip, COMMON_CLOCK_SGMII);
+		tc956x_common_clock_enable(chip, COMMON_CLOCK_REFCLK);
+		tc956x_clock_enable(chip, id, MAC_CLOCK_125M);
+		tc956x_clock_enable(chip, id, MAC_CLOCK_312_5M);
+	}
+
+	ret = tc956x_mac_configure(td, plat->phy_interface, plat->max_speed);
 	if (ret)
-		return ret;
+		dev_warn(td->dev, "failed to configure MAC speed: %d\n", ret);
 
 	tc956x_reset_deassert(chip, id, MAC_RESET_MAC);
 	tc956x_pma_init(td);
@@ -356,6 +523,7 @@ static int tc956x_mac_enable(struct tc956x_data *td)
 static void tc956x_mac_disable(struct tc956x_data *td)
 {
 	const struct tc956x_chip *chip = td->auxbus_data->chip;
+	struct plat_stmmacenet_data *plat = td->plat;
 	u32 id = td->auxbus_data->mac_id;
 
 	tc956x_reset_assert(chip, id, MAC_RESET_MAC);
@@ -367,6 +535,11 @@ static void tc956x_mac_disable(struct tc956x_data *td)
 	tc956x_clock_disable(chip, id, MAC_CLOCK_TX);
 	if (id)
 		tc956x_clock_disable(chip, id, MAC_CLOCK_RMII);
+
+	if (plat->phy_interface == PHY_INTERFACE_MODE_USXGMII) {
+		tc956x_clock_disable(chip, id, MAC_CLOCK_125M);
+		tc956x_clock_disable(chip, id, MAC_CLOCK_312_5M);
+	}
 }
 
 static void tc956x_mac_init_state(struct tc956x_data *td)
@@ -452,6 +625,13 @@ static int tc956x_mac_setup(void *apriv, struct mac_device_info *mac)
 
 	td = priv->plat->bsp_priv;
 
+	/* 10G USXGMII traffic needs the maximum descriptor ring to avoid
+	 * starving the DMA path. Keep this TC956X-specific instead of raising
+	 * stmmac's global defaults for every platform.
+	 */
+	priv->dma_conf.dma_rx_size = DMA_MAX_RX_SIZE;
+	priv->dma_conf.dma_tx_size = DMA_MAX_TX_SIZE;
+
 	/* dwxgmac301_dma_ops needs extending to provide DMA address translation */
 	dma = &td->dma;
 	*dma = dwxgmac301_dma_ops;
@@ -507,8 +687,12 @@ static struct phylink_pcs *tc956x_select_pcs(struct stmmac_priv *priv,
 static void tc956x_fix_mac_speed(void *bsp_priv, int speed, unsigned int mode)
 {
 	struct tc956x_data *td = bsp_priv;
+	phy_interface_t interface = td->plat->phy_interface;
+
+	tc956x_mac_configure(td, interface, speed);
+	if (interface == PHY_INTERFACE_MODE_USXGMII)
+		return;
 
-	tc956x_mac_configure(td, speed);
 	tc956x_pma_init(td);
 }
 
@@ -555,9 +739,35 @@ static int tc956x_dwmac_parse_dt(struct tc956x_data *td)
 	return 0;
 }
 
+static void tc956x_deassert_phy_resets(struct tc956x_data *td)
+{
+	struct device_node *child;
+	struct gpio_desc *reset;
+	u32 delay_us;
+
+	if (!td->plat->mdio_node)
+		return;
+
+	for_each_available_child_of_node(td->plat->mdio_node, child) {
+		reset = fwnode_gpiod_get_index(of_fwnode_handle(child), "reset", 0,
+						   GPIOD_OUT_LOW, "phy-reset");
+		if (IS_ERR(reset))
+			continue;
+
+		gpiod_set_value_cansleep(reset, 0);
+		gpiod_put(reset);
+
+		if (!of_property_read_u32(child, "reset-deassert-us", &delay_us))
+			fsleep(delay_us);
+	}
+}
+
 static int tc956x_lookup_max_speed(phy_interface_t phy_interface)
 {
 	switch (phy_interface) {
+	case PHY_INTERFACE_MODE_USXGMII:
+		return SPEED_10000;
+
 	case PHY_INTERFACE_MODE_SGMII:
 	case PHY_INTERFACE_MODE_2500BASEX:
 		return SPEED_2500;
@@ -593,7 +803,7 @@ static int tc956x_plat_dat_init(struct tc956x_data *td)
 	speed = ret;
 
 	plat->core_type = DWMAC_CORE_XGMAC;
-	plat->bus_id = td->auxbus_data->mac_id;
+	plat->bus_id = to_auxiliary_dev(dev)->id;
 	plat->phy_interface = phy_interface;
 	plat->mdio_bus_data = &td->mdio_bus_data;
 	/* Parent PCI device is used for DMA */
@@ -607,39 +817,37 @@ static int tc956x_plat_dat_init(struct tc956x_data *td)
 	 * represents "divide by 62" and gets the best rate under 2.5 MHz.
 	 */
 	plat->clk_csr = 0;	/* MDC clock = clk_csr_i / 62 */
-	plat->force_sf_dma_mode = 1;
+	plat->mdio_bus_data->default_an_inband =
+		phy_interface != PHY_INTERFACE_MODE_USXGMII;
+	plat->force_sf_dma_mode = true;
 	plat->max_speed = speed;
 	plat->unicast_filter_entries = 32;
 
 	/*
-	 * TC956x has 8 RX queues but we observe significantly reduced RX
-	 * bandwidth if we don't have at least 8k FIFO space per queue, so
-	 * by default we avoid using all the queues.
+	 * Use a single ordinary RX/TX queue and give it the whole FIFO partition.
+	 * TC956X partitions FIFO per function; exposing multiple stmmac queues makes
+	 * the core split this budget and leaves the active queue too small for 10G.
 	 */
-	plat->rx_queues_to_use = 4;
-
-	/*
-	 * TX956x has 8 TX queues but only #0 to #3 work for general IP traffic.
-	 * For now we will limit the driver to only these queues.
-	 */
-	plat->tx_queues_to_use = 4;
+	plat->rx_queues_to_use = TC956X_RX_QUEUES;
+	plat->tx_queues_to_use = TC956X_TX_QUEUES;
+	plat->rss_en = TC956X_RX_QUEUES > 1;
 
 	/*
 	 * Oversized FIFOs result in reduced performance in bandwidth tests.
 	 * Limit them to 8KiB per queue, or the total available.
 	 */
-	plat->tx_fifo_size =
-		min(TC956X_TX_FIFO_KB, 8 * plat->tx_queues_to_use) * SZ_1K;
-	plat->rx_fifo_size =
-		min(TC956X_RX_FIFO_KB, 8 * plat->rx_queues_to_use) * SZ_1K;
+	plat->tx_fifo_size = TC956X_TX_FIFO_KB * SZ_1K;
+	plat->rx_fifo_size = TC956X_RX_FIFO_KB * SZ_1K;
 	plat->host_dma_width = 36;
 
 	plat->rx_sched_algorithm = MTL_RX_ALGORITHM_SP;
 	plat->tx_sched_algorithm = MTL_TX_ALGORITHM_WRR;
 
 	/* Default RX chan is set to queue index (0..rx_queues_to_use-1) */
-	for (i = 0; i < plat->rx_queues_to_use; i++)
+	for (i = 0; i < plat->rx_queues_to_use; i++) {
 		plat->rx_queues_cfg[i].mode_to_use = MTL_QUEUE_DCB;
+		plat->rx_queues_cfg[i].chan = i;
+	}
 
 	for (i = 0; i < plat->tx_queues_to_use; i++) {
 		plat->tx_queues_cfg[i].weight = 12;
@@ -659,10 +867,11 @@ static int tc956x_plat_dat_init(struct tc956x_data *td)
 
 	plat->bsp_priv = td;
 	plat->clk_ptp_rate = TC956X_PTP_CLOCK_RATE;
+	plat->clk_ref_rate = TC956X_MAC_CLOCK_RATE;
 
 	/* AXI Configuration */
 	axi = &td->axi;
-	axi->axi_lpi_en = 1;
+	axi->axi_lpi_en = 0;
 	axi->axi_wr_osr_lmt = 31;
 	axi->axi_rd_osr_lmt = 31;
 	/* All sizes (2^2..2^8) are supported */
@@ -670,7 +879,7 @@ static int tc956x_plat_dat_init(struct tc956x_data *td)
 	plat->axi = axi;
 
 	plat->mac_port_sel_speed = speed;
-	plat->flags = STMMAC_FLAG_MULTI_MSI_EN | STMMAC_FLAG_TSO_EN;
+	plat->flags = STMMAC_FLAG_MULTI_MSI_EN;
 
 	td->plat = plat;
 
@@ -740,6 +949,8 @@ static int tc956x_dwmac_probe(struct auxiliary_device *adev,
 
 	/* Put the MAC in a known initial state */
 	tc956x_mac_init_state(td);
+	tc956x_mac_enable(td);
+	tc956x_deassert_phy_resets(td);
 
 	ret = tc956x_stmmac_resources_init(td, &res);
 	if (ret)
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index a7bb2f3e02047..abea939fcf967 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -6390,6 +6390,7 @@ static int stmmac_setup_tc(struct net_device *ndev, enum tc_setup_type type,
 static u16 stmmac_select_queue(struct net_device *dev, struct sk_buff *skb,
 			       struct net_device *sb_dev)
 {
+	struct stmmac_priv *priv = netdev_priv(dev);
 	int gso = skb_shinfo(skb)->gso_type;
 
 	if (gso & (SKB_GSO_TCPV4 | SKB_GSO_TCPV6 | SKB_GSO_UDP_L4)) {
@@ -6399,7 +6400,8 @@ static u16 stmmac_select_queue(struct net_device *dev, struct sk_buff *skb,
 		 * because if TSO/USO is supported then at least this
 		 * one will be capable.
 		 */
-		return 0;
+		if (priv->dma_cap.tsoen && priv->plat->flags & STMMAC_FLAG_TSO_EN)
+			return 0;
 	}
 
 	return netdev_pick_tx(dev, skb, NULL) % dev->real_num_tx_queues;
diff --git a/drivers/net/pcs/pcs-xpcs.c b/drivers/net/pcs/pcs-xpcs.c
index 1d62d5b31c61c..ce09cc0ce2f54 100644
--- a/drivers/net/pcs/pcs-xpcs.c
+++ b/drivers/net/pcs/pcs-xpcs.c
@@ -27,6 +27,11 @@ static const int xpcs_usxgmii_features[] = {
 	ETHTOOL_LINK_MODE_10000baseKX4_Full_BIT,
 	ETHTOOL_LINK_MODE_10000baseKR_Full_BIT,
 	ETHTOOL_LINK_MODE_2500baseX_Full_BIT,
+	ETHTOOL_LINK_MODE_100baseT_Full_BIT,
+	ETHTOOL_LINK_MODE_1000baseT_Full_BIT,
+	ETHTOOL_LINK_MODE_2500baseT_Full_BIT,
+	ETHTOOL_LINK_MODE_5000baseT_Full_BIT,
+	ETHTOOL_LINK_MODE_10000baseT_Full_BIT,
 	__ETHTOOL_LINK_MODE_MASK_NBITS,
 };
 
@@ -705,46 +710,89 @@ static void xpcs_get_interfaces(struct dw_xpcs *xpcs, unsigned long *interfaces)
 static int xpcs_switch_interface_mode(struct dw_xpcs *xpcs,
 				      phy_interface_t interface)
 {
-	int ret = 0;
+	int mdio_stat2, ret;
 
-	if (xpcs->info.pma == WX_TXGBE_XPCS_PMA_10G_ID) {
-		ret = txgbe_xpcs_switch_mode(xpcs, interface);
-	} else if (xpcs->interface != interface) {
-		if (interface == PHY_INTERFACE_MODE_SGMII)
-			xpcs->need_reset = true;
-		xpcs->interface = interface;
+	/* Wangxun provides a full alternative implementation to handle quirks */
+	if (xpcs->info.pma == WX_TXGBE_XPCS_PMA_10G_ID)
+		return txgbe_xpcs_switch_mode(xpcs, interface);
+
+	mdio_stat2 = xpcs_read(xpcs, MDIO_MMD_PCS, MDIO_STAT2);
+	if (mdio_stat2 < 0)
+		return mdio_stat2;
+
+	/*
+	 * If this XPCS supports 10Gbase-R then that will be the default
+	 * operating mode. There are several interface modes where this default
+	 * is unhelpful. Change the operating mode for interfaces were we know
+	 * the default is wrong, and restore the default otherwise.
+	 */
+	if (mdio_stat2 & MDIO_PCS_STAT2_10GBR) {
+		switch (interface) {
+		case PHY_INTERFACE_MODE_SGMII:
+		case PHY_INTERFACE_MODE_1000BASEX:
+		case PHY_INTERFACE_MODE_2500BASEX:
+			/*
+			 * Why are we writing MDIO_PCS_CTRL2_TYPE + 1? We want
+			 * the modal behaviour that comes when we pick a
+			 * reserved value. XPCS allocates extra bits to this
+			 * field and allocates values from 15 down so
+			 * MDIO_PCS_CTRL2_TYPE + 1 is the value likely to be
+			 * allocated last (and hopefully never).
+			 */
+			ret = xpcs_write(xpcs, MDIO_MMD_PCS, MDIO_CTRL2,
+					 MDIO_PCS_CTRL2_TYPE + 1);
+			if (ret < 0)
+				return ret;
+			break;
+		default:
+			ret = xpcs_write(xpcs, MDIO_MMD_PCS, MDIO_CTRL2,
+					 MDIO_PCS_CTRL2_10GBR);
+			if (ret < 0)
+				return ret;
+			break;
+		}
 	}
 
-	return ret;
+	xpcs->interface = interface;
+
+	return 0;
 }
 
 static void xpcs_pre_config(struct phylink_pcs *pcs, phy_interface_t interface)
 {
 	struct dw_xpcs *xpcs = phylink_pcs_to_xpcs(pcs);
 	const struct dw_xpcs_compat *compat;
+	bool force_reset;
 	int ret;
 
-	ret = xpcs_switch_interface_mode(xpcs, interface);
-	if (ret)
-		dev_err(&xpcs->mdiodev->dev, "switch interface failed: %pe\n",
-			ERR_PTR(ret));
+	/*
+	 * According to the XPCS datasheet, a soft reset is required to initiate
+	 * Clause 37 auto-negotiation when the XPCS switches interface modes.
+	 */
+	force_reset = interface == PHY_INTERFACE_MODE_SGMII;
 
-	if (!xpcs->need_reset)
-		return;
+	if (force_reset || xpcs->need_reset) {
+		compat = xpcs_find_compat(xpcs, interface);
+		if (!compat) {
+			dev_err(&xpcs->mdiodev->dev, "unsupported interface %s\n",
+				phy_modes(interface));
+			return;
+		}
 
-	compat = xpcs_find_compat(xpcs, interface);
-	if (!compat) {
-		dev_err(&xpcs->mdiodev->dev, "unsupported interface %s\n",
-			phy_modes(interface));
-		return;
+		ret = xpcs_soft_reset(xpcs, compat);
+		if (ret) {
+			dev_err(&xpcs->mdiodev->dev, "soft reset failed: %pe\n",
+				ERR_PTR(ret));
+			return;
+		}
+
+		xpcs->need_reset = false;
 	}
 
-	ret = xpcs_soft_reset(xpcs, compat);
+	ret = xpcs_switch_interface_mode(xpcs, interface);
 	if (ret)
-		dev_err(&xpcs->mdiodev->dev, "soft reset failed: %pe\n",
+		dev_err(&xpcs->mdiodev->dev, "switch interface failed: %pe\n",
 			ERR_PTR(ret));
-
-	xpcs->need_reset = false;
 }
 
 static int xpcs_config_operating_mode(struct dw_xpcs *xpcs, int an_mode)
@@ -942,6 +990,44 @@ static int xpcs_config_2500basex(struct dw_xpcs *xpcs)
 			   BMCR_SPEED1000);
 }
 
+static int xpcs_config_usxgmii(struct dw_xpcs *xpcs)
+{
+	int ret, val;
+
+	/* Select the 10GBASE-R PCS type used as the base for USXGMII */
+	ret = xpcs_modify(xpcs, MDIO_MMD_PCS, MDIO_CTRL2,
+			  MDIO_PCS_CTRL2_TYPE, MDIO_PCS_CTRL2_10GBR);
+	if (ret < 0)
+		return ret;
+
+	/* Enable USXGMII and select the 10G USXGMII baud mode. The DW XPCS
+	 * does not do this on its own for USXGMII, and without it the SerDes
+	 * block-locks but no USXGMII frames are decoded (RX stays dead).
+	 */
+	ret = xpcs_modify_vpcs(xpcs, DW_VR_XS_PCS_DIG_CTRL1,
+			       DW_USXGMII_EN, DW_USXGMII_EN);
+	if (ret < 0)
+		return ret;
+
+	ret = xpcs_modify_vpcs(xpcs, DW_VR_XS_PCS_KR_CTRL,
+			       DW_USXG_MODE, DW_USXG_MODE_10G);
+	if (ret < 0)
+		return ret;
+
+	/* Apply the configuration with a vendor soft reset and wait for it
+	 * to self-clear.
+	 */
+	ret = xpcs_modify_vpcs(xpcs, DW_VR_XS_PCS_DIG_CTRL1,
+			       DW_VR_RST, DW_VR_RST);
+	if (ret < 0)
+		return ret;
+
+	return read_poll_timeout(xpcs_read_vpcs, val,
+				 val < 0 || !(val & DW_VR_RST),
+				 1000, 50000, false,
+				 xpcs, DW_VR_XS_PCS_DIG_CTRL1);
+}
+
 static int xpcs_do_config(struct dw_xpcs *xpcs, phy_interface_t interface,
 			  const unsigned long *advertising,
 			  unsigned int neg_mode)
@@ -953,6 +1039,12 @@ static int xpcs_do_config(struct dw_xpcs *xpcs, phy_interface_t interface,
 	if (!compat)
 		return -ENODEV;
 
+	if (interface == PHY_INTERFACE_MODE_USXGMII) {
+		ret = xpcs_config_usxgmii(xpcs);
+		if (ret < 0)
+			return ret;
+	}
+
 	ret = xpcs_config_operating_mode(xpcs, compat->an_mode);
 	if (ret < 0)
 		return ret;
diff --git a/drivers/net/pcs/pcs-xpcs.h b/drivers/net/pcs/pcs-xpcs.h
index 929fa238445ed..484981cf5d53c 100644
--- a/drivers/net/pcs/pcs-xpcs.h
+++ b/drivers/net/pcs/pcs-xpcs.h
@@ -24,6 +24,11 @@
 #define DW_PSEQ_ST			GENMASK(4, 2)
 #define DW_PSEQ_ST_GOOD			FIELD_PREP(GENMASK(4, 2), 0x4)
 
+/* VR_XS_PCS_KR_CTRL - selects the USXGMII baud/mode (10G/5G/2.5G) */
+#define DW_VR_XS_PCS_KR_CTRL		0x001c
+#define DW_USXG_MODE			GENMASK(12, 10)
+#define DW_USXG_MODE_10G		0x0
+
 /* SR_MII */
 #define DW_USXGMII_FULL			BIT(8)
 #define DW_USXGMII_SS_MASK		(BIT(13) | BIT(6) | BIT(5))
diff --git a/drivers/net/phy/as21xxx.c b/drivers/net/phy/as21xxx.c
index d5738117eca68..76a7b495a3360 100644
--- a/drivers/net/phy/as21xxx.c
+++ b/drivers/net/phy/as21xxx.c
@@ -7,6 +7,7 @@
 
 #include <linux/bitfield.h>
 #include <linux/firmware.h>
+#include <linux/iopoll.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/phy.h>
@@ -21,6 +22,9 @@
 							 BIT(_n))
 
 #define VEND1_FW_START_ADDR		0x100
+#define AS22XXX_AN_STATES1		0x8005
+#define AS22XXX_AN_STATES1_ARB_MASK	GENMASK(15, 12)
+#define AS22XXX_LINK_GOOD		9
 
 #define VEND1_GLB_REG_MDIO_INDIRECT_ADDRCMD 0x101
 #define VEND1_GLB_REG_MDIO_INDIRECT_LOAD 0x102
@@ -139,6 +143,7 @@
 #define PHY_ID_AS21510PB1		0x75009472
 #define PHY_ID_AS21210JB1		0x75009482
 #define PHY_ID_AS21210PB1		0x75009492
+#define PHY_ID_AS22XXX			0x750094a1
 #define PHY_VENDOR_AEONSEMI		0x75009400
 
 #define AEON_MAX_LEDS			5
@@ -768,6 +773,139 @@ static int as21xxx_read_status(struct phy_device *phydev)
 	return 0;
 }
 
+static int as22xxx_read_link(struct phy_device *phydev, int *bmcr)
+{
+	int status = 0;
+	bool link_up;
+
+	*bmcr = phy_read_mmd(phydev, MDIO_MMD_AN,
+			     AS21XXX_MDIO_AN_C22 + MII_BMCR);
+	if (*bmcr < 0)
+		return *bmcr;
+
+	if (*bmcr & BMCR_ANRESTART)
+		goto done;
+
+	status = phy_read_mmd(phydev, MDIO_MMD_AN, AS22XXX_AN_STATES1);
+	if (status < 0)
+		return status;
+
+done:
+	link_up = FIELD_GET(AS22XXX_AN_STATES1_ARB_MASK, status) ==
+		  AS22XXX_LINK_GOOD;
+	phydev->link = link_up;
+	phydev->autoneg_complete = link_up;
+
+	if (phydev->autoneg == AUTONEG_ENABLE && !phydev->autoneg_complete)
+		phydev->link = 0;
+
+	return 0;
+}
+
+static int as22xxx_read_lpa(struct phy_device *phydev)
+{
+	int lpa, ret;
+
+	if (phydev->autoneg == AUTONEG_ENABLE && !phydev->autoneg_complete) {
+		mii_stat1000_mod_linkmode_lpa_t(phydev->lp_advertising, 0);
+		mii_lpa_mod_linkmode_lpa_t(phydev->lp_advertising, 0);
+		return 0;
+	}
+
+	ret = as21xxx_read_c22_lpa(phydev);
+	if (ret)
+		return ret;
+
+	lpa = phy_read_mmd(phydev, MDIO_MMD_AN,
+			   AS21XXX_MDIO_AN_C22 + MII_LPA);
+	if (lpa < 0)
+		return lpa;
+
+	mii_lpa_mod_linkmode_lpa_t(phydev->lp_advertising, lpa);
+
+	lpa = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_10GBT_STAT);
+	if (lpa < 0)
+		return lpa;
+
+	mii_10gbt_stat_mod_linkmode_lpa_t(phydev->lp_advertising, lpa);
+
+	return 0;
+}
+
+static void as22xxx_read_speed(struct phy_device *phydev, int bmcr)
+{
+	int speed;
+
+	speed = phy_read_mmd(phydev, MDIO_MMD_VEND1, VEND1_SPEED_STATUS);
+	if (speed < 0)
+		return;
+
+	speed &= VEND1_SPEED_MASK;
+	if (speed == VEND1_SPEED_10000) {
+		phydev->speed = SPEED_10000;
+		phydev->duplex = DUPLEX_FULL;
+	} else if (speed == VEND1_SPEED_5000) {
+		phydev->speed = SPEED_5000;
+		phydev->duplex = DUPLEX_FULL;
+	} else if (speed == VEND1_SPEED_2500) {
+		phydev->speed = SPEED_2500;
+		phydev->duplex = DUPLEX_FULL;
+	} else if (speed == VEND1_SPEED_1000) {
+		phydev->speed = SPEED_1000;
+		if (bmcr & BMCR_FULLDPLX)
+			phydev->duplex = DUPLEX_FULL;
+		else
+			phydev->duplex = DUPLEX_HALF;
+	} else if (speed == VEND1_SPEED_100) {
+		phydev->speed = SPEED_100;
+		if (bmcr & BMCR_FULLDPLX)
+			phydev->duplex = DUPLEX_FULL;
+		else
+			phydev->duplex = DUPLEX_HALF;
+	} else {
+		phydev->speed = SPEED_10;
+		phydev->duplex = DUPLEX_FULL;
+	}
+}
+
+static int as22xxx_read_status(struct phy_device *phydev)
+{
+	int bmcr, old_link = phydev->link;
+	int ret;
+
+	ret = as22xxx_read_link(phydev, &bmcr);
+	if (ret)
+		return ret;
+
+	if (phydev->autoneg == AUTONEG_ENABLE && old_link && phydev->link)
+		return 0;
+
+	phydev->speed = SPEED_UNKNOWN;
+	phydev->duplex = DUPLEX_UNKNOWN;
+	phydev->pause = 0;
+	phydev->asym_pause = 0;
+
+	if (phydev->autoneg == AUTONEG_ENABLE) {
+		ret = genphy_c45_read_lpa(phydev);
+		if (ret)
+			return ret;
+
+		ret = as22xxx_read_lpa(phydev);
+		if (ret)
+			return ret;
+
+		if (phydev->autoneg_complete) {
+			as22xxx_read_speed(phydev, bmcr);
+			phy_resolve_aneg_linkmode(phydev);
+		}
+	} else {
+		linkmode_zero(phydev->lp_advertising);
+		as22xxx_read_speed(phydev, bmcr);
+	}
+
+	return 0;
+}
+
 static int as21xxx_led_brightness_set(struct phy_device *phydev,
 				      u8 index, enum led_brightness value)
 {
@@ -943,6 +1081,71 @@ static int as21xxx_match_phy_device(struct phy_device *phydev,
 	return ret;
 }
 
+static int as22xxx_match_phy_device(struct phy_device *phydev,
+				    const struct phy_driver *phydrv)
+{
+	u32 phy_id;
+	int ret;
+
+	ret = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MII_PHYSID1);
+	if (ret < 0)
+		return ret;
+	phy_id = ret << 16;
+
+	ret = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MII_PHYSID2);
+	if (ret < 0)
+		return ret;
+	phy_id |= ret;
+
+	return phy_id == PHY_ID_AS22XXX;
+}
+
+static int as22xxx_probe(struct phy_device *phydev)
+{
+	struct as21xxx_priv *priv;
+	int ret;
+
+	/* Gen2 keeps its generic PHY ID before and after firmware load and may
+	 * expose only PMA/PMD initially. Force the MMD bitmap used by generic C45
+	 * helpers, then load firmware from probe and wait for IPC to come alive.
+	 */
+	phydev->c45_ids.mmds_present |= MDIO_DEVS_PMAPMD | MDIO_DEVS_PCS |
+					MDIO_DEVS_AN;
+
+	priv = devm_kzalloc(&phydev->mdio.dev,
+			    sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+	phydev->priv = priv;
+
+	ret = devm_mutex_init(&phydev->mdio.dev,
+			      &priv->ipc_lock);
+	if (ret)
+		return ret;
+
+	ret = aeon_firmware_load(phydev);
+	if (ret)
+		return ret;
+
+	ret = read_poll_timeout(aeon_ipc_sync_parity, ret, !ret,
+				0, 10000000, false, phydev, priv);
+	if (ret) {
+		phydev_err(phydev, "timed out waiting for firmware boot\n");
+		return ret;
+	}
+
+	ret = aeon_ipc_get_fw_version(phydev);
+	if (ret)
+		return ret;
+
+	ret = phy_set_bits_mmd(phydev, MDIO_MMD_VEND1, VEND1_PTP_CLK,
+			       VEND1_PTP_CLK_EN);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
 static struct phy_driver as21xxx_drivers[] = {
 	{
 		/* PHY expose in C45 as 0x7500 0x9410
@@ -1074,6 +1277,18 @@ static struct phy_driver as21xxx_drivers[] = {
 		.led_hw_control_get = as21xxx_led_hw_control_get,
 		.led_polarity_set = as21xxx_led_polarity_set,
 	},
+	{
+		PHY_ID_MATCH_EXACT(PHY_ID_AS22XXX),
+		.name		= "Aeonsemi AS22XXX",
+		.probe		= as22xxx_probe,
+		.match_phy_device = as22xxx_match_phy_device,
+		.read_status	= as22xxx_read_status,
+		.led_brightness_set = as21xxx_led_brightness_set,
+		.led_hw_is_supported = as21xxx_led_hw_is_supported,
+		.led_hw_control_set = as21xxx_led_hw_control_set,
+		.led_hw_control_get = as21xxx_led_hw_control_get,
+		.led_polarity_set = as21xxx_led_polarity_set,
+	},
 };
 module_phy_driver(as21xxx_drivers);
 
diff --git a/include/soc/toshiba/tc956x-dwmac.h b/include/soc/toshiba/tc956x-dwmac.h
index 5ca39cf764be9..5840c1a9a51d7 100644
--- a/include/soc/toshiba/tc956x-dwmac.h
+++ b/include/soc/toshiba/tc956x-dwmac.h
@@ -32,13 +32,21 @@ enum tc956x_clock_id {
 	MAC_CLOCK_RMII		= 15,	/* eMAC 1 only */
 };
 
+enum tc956x_common_clock_id {
+	COMMON_CLOCK_PLL	= 24,	/* POEPLLCEN */
+	COMMON_CLOCK_SGMII	= 25,	/* SGMPCIEN */
+	COMMON_CLOCK_REFCLK	= 26,	/* REFCLKOCEN (PHY refclk output) */
+};
+
 /**
  * struct tc956x_dwmac_data - Structure passed to stmmac auxiliary devices.
  * @chip:		Context pointer needed for reset and clock operations
  * @emac:		I/O mapped address used by eMAC
  * @emac_ctl:		I/O mapped address used for eMAC control
+ * @sfr:		I/O mapped address used for TC956X SFR access
  * @msigen:		I/O mapped address used by MSIGEN
- * @msigen_irq:		IRQ number used by MSIGEN
+ * @msigen_irq:		IRQ number of the first MSIGEN vector
+ * @msigen_nvec:	Number of consecutive MSIGEN vectors from @msigen_irq
  * @rev_id:		Chip revision ID (for quirks)
  * @mac_id:		Unique device ID (0 or 1)
  *
@@ -48,8 +56,10 @@ struct tc956x_dwmac_data {
 	const struct tc956x_chip *chip;
 	void __iomem *emac;
 	void __iomem *emac_ctl;
+	void __iomem *sfr;
 	void __iomem *msigen;
 	unsigned int msigen_irq;
+	unsigned int msigen_nvec;
 	u8 rev_id;
 	u8 mac_id;
 };
@@ -81,4 +91,16 @@ static inline void tc956x_clock_disable(const struct tc956x_chip *chip,
 	tc956x_reset_clock_set(chip, false, !mac_id, false, (u8)id);
 }
 
+static inline void tc956x_common_clock_enable(const struct tc956x_chip *chip,
+					      enum tc956x_common_clock_id id)
+{
+	tc956x_reset_clock_set(chip, false, true, true, (u8)id);
+}
+
+static inline void tc956x_common_clock_disable(const struct tc956x_chip *chip,
+					       enum tc956x_common_clock_id id)
+{
+	tc956x_reset_clock_set(chip, false, true, false, (u8)id);
+}
+
 #endif /* __TOSHIBA_TC956X_DWMAC_H__*/