diff mbox series

[SRU,F:linux-bluefield,v1,1/1] UBUNTU: SAUCE: Sync up mlxbf-gige driver with upstreamed version

Message ID 20210707180016.21351-2-asmaa@nvidia.com
State New
Headers show
Series UBUNTU: SAUCE: Sync up mlxbf-gige with upstreamed version | expand

Commit Message

Asmaa Mnebhi July 7, 2021, 6 p.m. UTC
BugLink: https://bugs.launchpad.net/bugs/1934923

The mlxbf-gige driver has just been upstreamed so linux-bluefield needs to be synced up with what we have upstreamed.

Reviewed-by: David Thompson <davthompson@nvidia.com>
Signed-off-by: Asmaa Mnebhi <asmaa@nvidia.com>
---
 drivers/gpio/gpio-mlxbf2.c                    |  42 +---
 .../net/ethernet/mellanox/mlxbf_gige/Kconfig  |   2 +-
 .../net/ethernet/mellanox/mlxbf_gige/Makefile |  10 +-
 .../ethernet/mellanox/mlxbf_gige/mlxbf_gige.h |  20 +-
 .../mellanox/mlxbf_gige/mlxbf_gige_ethtool.c  |  11 +-
 .../mellanox/mlxbf_gige/mlxbf_gige_gpio.c     | 212 ++++++++++++++++++
 .../mellanox/mlxbf_gige/mlxbf_gige_intr.c     |   3 +-
 .../mellanox/mlxbf_gige/mlxbf_gige_main.c     |  72 +++---
 .../mellanox/mlxbf_gige/mlxbf_gige_mdio.c     |   2 +-
 .../mellanox/mlxbf_gige/mlxbf_gige_regs.h     |   2 +-
 .../mellanox/mlxbf_gige/mlxbf_gige_rx.c       |   8 +-
 .../mellanox/mlxbf_gige/mlxbf_gige_tx.c       |   5 +-
 12 files changed, 284 insertions(+), 105 deletions(-)
 create mode 100644 drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_gpio.c
diff mbox series

Patch

diff --git a/drivers/gpio/gpio-mlxbf2.c b/drivers/gpio/gpio-mlxbf2.c
index e7f17765ff11..905fe7766e19 100644
--- a/drivers/gpio/gpio-mlxbf2.c
+++ b/drivers/gpio/gpio-mlxbf2.c
@@ -21,7 +21,7 @@ 
 #include <linux/spinlock.h>
 #include <linux/types.h>
 
-#define DRV_VERSION "1.3"
+#define DRV_VERSION "1.4"
 
 /*
  * There are 3 YU GPIO blocks:
@@ -98,9 +98,6 @@  struct mlxbf2_gpio_context {
 	/* YU GPIO pin responsible for soft reset */
 	unsigned long rst_pin;
 
-	/* YU GPIO pin connected to PHY INT_N signal */
-	unsigned long phy_int_pin;
-
 	/* YU GPIO block interrupt mask */
 	u32 gpio_int_mask;
 
@@ -343,8 +340,6 @@  static u32 mlxbf2_gpio_get_int_mask(struct mlxbf2_gpio_context *gs)
 	/*
 	 * Determine bit mask within the yu gpio block.
 	 */
-	if (gs->phy_int_pin != MLXBF2_GPIO_MAX_PINS_PER_BLOCK)
-		gpio_int_mask = BIT(gs->phy_int_pin);
 	if (gs->rst_pin != MLXBF2_GPIO_MAX_PINS_PER_BLOCK)
 		gpio_int_mask |= BIT(gs->rst_pin);
 	if (gs->low_pwr_pin != MLXBF2_GPIO_MAX_PINS_PER_BLOCK)
@@ -391,9 +386,6 @@  static irqreturn_t mlxbf2_gpio_irq_handler(int irq, void *ptr)
 	val |= gpio_pin;
 	writel(val, gs->gpio_io + YU_GPIO_CAUSE_OR_CLRCAUSE);
 
-	if ((gpio_block & BIT(GPIO_BLOCK0)) && (gpio_pin & BIT(gs->phy_int_pin)))
-		generic_handle_irq(irq_find_mapping(gs->gc.irq.domain, gs->phy_int_pin));
-
 	spin_unlock_irqrestore(&gs->gc.bgpio_lock, flags);
 
 	if (gpio_block & BIT(GPIO_BLOCK16)) {
@@ -465,15 +457,6 @@  static void mlxbf2_gpio_disable_int(struct mlxbf2_gpio_context *gs)
 	spin_unlock_irqrestore(&gs->gc.bgpio_lock, flags);
 }
 
-static int mlxbf2_gpio_to_irq(struct gpio_chip *chip, unsigned gpio)
-{
-	struct mlxbf2_gpio_context *gs;
-
-	gs = gpiochip_get_data(chip);
-
-	return irq_create_mapping(gs->gc.irq.domain, gpio);
-}
-
 /* BlueField-2 GPIO driver initialization routine. */
 static int
 mlxbf2_gpio_probe(struct platform_device *pdev)
@@ -482,7 +465,6 @@  mlxbf2_gpio_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct gpio_irq_chip *girq;
 	unsigned int low_pwr_pin;
-	unsigned int phy_int_pin;
 	unsigned int rst_pin;
 	struct gpio_chip *gc;
 	struct resource *res;
@@ -541,14 +523,6 @@  mlxbf2_gpio_probe(struct platform_device *pdev)
 	gc->direction_output = mlxbf2_gpio_direction_output;
 	gc->ngpio = npins;
 	gc->owner = THIS_MODULE;
-	gc->to_irq = mlxbf2_gpio_to_irq;
-
-	/*
-	 * PHY interrupt
-	 */
-	ret = device_property_read_u32(dev, "phy-int-pin", &phy_int_pin);
-	if (ret < 0)
-		phy_int_pin = MLXBF2_GPIO_MAX_PINS_PER_BLOCK;
 
 	/*
 	 * OCP3.0 supports the low power mode interrupt.
@@ -564,7 +538,6 @@  mlxbf2_gpio_probe(struct platform_device *pdev)
 	if (ret < 0)
 		rst_pin = MLXBF2_GPIO_MAX_PINS_PER_BLOCK;
 
-	gs->phy_int_pin = phy_int_pin;
 	gs->low_pwr_pin = low_pwr_pin;
 	gs->rst_pin = rst_pin;
 	gs->gpio_int_mask = mlxbf2_gpio_get_int_mask(gs);
@@ -602,12 +575,6 @@  mlxbf2_gpio_probe(struct platform_device *pdev)
 	}
 	platform_set_drvdata(pdev, gs);
 
-	if (phy_int_pin != MLXBF2_GPIO_MAX_PINS_PER_BLOCK) {
-		/* Create phy irq mapping */
-		mlxbf2_gpio_to_irq(&gs->gc, phy_int_pin);
-		/* Enable sharing the irq domain with the PHY driver */
-		irq_set_default_host(gs->gc.irq.domain);
-	}
 
 	return 0;
 }
@@ -619,14 +586,9 @@  mlxbf2_gpio_remove(struct platform_device *pdev)
 
 	gs = platform_get_drvdata(pdev);
 
-	if ((gs->phy_int_pin != MLXBF2_GPIO_MAX_PINS_PER_BLOCK) ||
-	    (gs->low_pwr_pin != MLXBF2_GPIO_MAX_PINS_PER_BLOCK) ||
-	    (gs->rst_pin != MLXBF2_GPIO_MAX_PINS_PER_BLOCK)) {
-		mlxbf2_gpio_disable_int(gs);
-	}
-
 	if ((gs->low_pwr_pin != MLXBF2_GPIO_MAX_PINS_PER_BLOCK) ||
 	    (gs->rst_pin != MLXBF2_GPIO_MAX_PINS_PER_BLOCK)) {
+		mlxbf2_gpio_disable_int(gs);
 		flush_work(&gs->send_work);
 	}
 
diff --git a/drivers/net/ethernet/mellanox/mlxbf_gige/Kconfig b/drivers/net/ethernet/mellanox/mlxbf_gige/Kconfig
index 0338ba5f46b6..4cdebafaf222 100644
--- a/drivers/net/ethernet/mellanox/mlxbf_gige/Kconfig
+++ b/drivers/net/ethernet/mellanox/mlxbf_gige/Kconfig
@@ -5,7 +5,7 @@ 
 
 config MLXBF_GIGE
 	tristate "Mellanox Technologies BlueField Gigabit Ethernet support"
-	depends on (ARM64 || COMPILE_TEST) && ACPI && GPIO_MLXBF2
+	depends on (ARM64 && ACPI) || COMPILE_TEST
 	select PHYLIB
 	help
 	  The second generation BlueField SoC from Mellanox Technologies
diff --git a/drivers/net/ethernet/mellanox/mlxbf_gige/Makefile b/drivers/net/ethernet/mellanox/mlxbf_gige/Makefile
index 6caae3ca41cf..e57c1375f236 100644
--- a/drivers/net/ethernet/mellanox/mlxbf_gige/Makefile
+++ b/drivers/net/ethernet/mellanox/mlxbf_gige/Makefile
@@ -2,6 +2,10 @@ 
 
 obj-$(CONFIG_MLXBF_GIGE) += mlxbf_gige.o
 
-mlxbf_gige-y := mlxbf_gige_ethtool.o mlxbf_gige_intr.o \
-		mlxbf_gige_main.o mlxbf_gige_mdio.o \
-		mlxbf_gige_rx.o mlxbf_gige_tx.o
+mlxbf_gige-y := mlxbf_gige_ethtool.o \
+		mlxbf_gige_gpio.o \
+		mlxbf_gige_intr.o \
+		mlxbf_gige_main.o \
+		mlxbf_gige_mdio.o \
+		mlxbf_gige_rx.o   \
+		mlxbf_gige_tx.o
diff --git a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige.h b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige.h
index 2c049bf114b1..e3509e69ed1c 100644
--- a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige.h
+++ b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige.h
@@ -4,7 +4,7 @@ 
  * - this file contains software data structures and any chip-specific
  *   data structures (e.g. TX WQE format) that are memory resident.
  *
- * Copyright (C) 2020-2021 Mellanox Technologies, Ltd. ALL RIGHTS RESERVED.
+ * Copyright (C) 2020-2021 NVIDIA CORPORATION & AFFILIATES
  */
 
 #ifndef __MLXBF_GIGE_H__
@@ -13,6 +13,7 @@ 
 #include <linux/io-64-nonatomic-lo-hi.h>
 #include <linux/irqreturn.h>
 #include <linux/netdevice.h>
+#include <linux/irq.h>
 
 /* The silicon design supports a maximum RX ring size of
  * 32K entries. Based on current testing this maximum size
@@ -80,8 +81,11 @@  struct mlxbf_gige {
 	struct platform_device *pdev;
 	void __iomem *mdio_io;
 	struct mii_bus *mdiobus;
-	spinlock_t lock;
-	spinlock_t gpio_lock;
+	void __iomem *gpio_io;
+	struct irq_domain *irqdomain;
+	u32 phy_int_gpio_mask;
+	spinlock_t lock;      /* for packet processing indices */
+	spinlock_t gpio_lock; /* for GPIO bus access */
 	u16 rx_q_entries;
 	u16 tx_q_entries;
 	u64 *tx_wqe_base;
@@ -104,13 +108,11 @@  struct mlxbf_gige {
 	int rx_irq;
 	int llu_plu_irq;
 	int phy_irq;
+	int hw_phy_irq;
 	bool promisc_enabled;
 	u8 valid_polarity;
 	struct napi_struct napi;
 	struct mlxbf_gige_stats stats;
-	u32 tx_pause;
-	u32 rx_pause;
-	u32 aneg_pause;
 };
 
 /* Rx Work Queue Element definitions */
@@ -136,7 +138,7 @@  struct mlxbf_gige {
 
 /* Macro to return packet length of specified TX WQE */
 #define MLXBF_GIGE_TX_WQE_PKT_LEN(tx_wqe_addr) \
-	(*(tx_wqe_addr + 1) & MLXBF_GIGE_TX_WQE_PKT_LEN_MASK)
+	(*((tx_wqe_addr) + 1) & MLXBF_GIGE_TX_WQE_PKT_LEN_MASK)
 
 /* Tx Completion Count */
 #define MLXBF_GIGE_TX_CC_SZ                    8
@@ -145,6 +147,7 @@  struct mlxbf_gige {
 enum mlxbf_gige_res {
 	MLXBF_GIGE_RES_MAC,
 	MLXBF_GIGE_RES_MDIO9,
+	MLXBF_GIGE_RES_GPIO0,
 	MLXBF_GIGE_RES_LLU,
 	MLXBF_GIGE_RES_PLU
 };
@@ -181,4 +184,7 @@  int mlxbf_gige_poll(struct napi_struct *napi, int budget);
 extern const struct ethtool_ops mlxbf_gige_ethtool_ops;
 void mlxbf_gige_update_tx_wqe_next(struct mlxbf_gige *priv);
 
+int mlxbf_gige_gpio_init(struct platform_device *pdev, struct mlxbf_gige *priv);
+void mlxbf_gige_gpio_free(struct mlxbf_gige *priv);
+
 #endif /* !defined(__MLXBF_GIGE_H__) */
diff --git a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_ethtool.c b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_ethtool.c
index 1d68b8ba9b02..24a32ffee4e4 100644
--- a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_ethtool.c
@@ -2,7 +2,7 @@ 
 
 /* Ethtool support for Mellanox Gigabit Ethernet driver
  *
- * Copyright (C) 2020-2021 Mellanox Technologies, Ltd. ALL RIGHTS RESERVED.
+ * Copyright (C) 2020-2021 NVIDIA CORPORATION & AFFILIATES
  */
 
 #include <linux/phy.h>
@@ -155,11 +155,9 @@  static void mlxbf_gige_get_ethtool_stats(struct net_device *netdev,
 static void mlxbf_gige_get_pauseparam(struct net_device *netdev,
 				      struct ethtool_pauseparam *pause)
 {
-	struct mlxbf_gige *priv = netdev_priv(netdev);
-
-	pause->autoneg = priv->aneg_pause;
-	pause->rx_pause = priv->rx_pause;
-	pause->tx_pause = priv->tx_pause;
+	pause->autoneg = AUTONEG_DISABLE;
+	pause->rx_pause = 1;
+	pause->tx_pause = 1;
 }
 
 const struct ethtool_ops mlxbf_gige_ethtool_ops = {
@@ -175,4 +173,3 @@  const struct ethtool_ops mlxbf_gige_ethtool_ops = {
 	.get_pauseparam		= mlxbf_gige_get_pauseparam,
 	.get_link_ksettings	= phy_ethtool_get_link_ksettings,
 };
-
diff --git a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_gpio.c b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_gpio.c
new file mode 100644
index 000000000000..a8d966db5715
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_gpio.c
@@ -0,0 +1,212 @@ 
+// SPDX-License-Identifier: GPL-2.0-only OR BSD-3-Clause
+
+/* Initialize and handle GPIO interrupt triggered by INT_N PHY signal.
+ * This GPIO interrupt triggers the PHY state machine to bring the link
+ * up/down.
+ *
+ * Copyright (C) 2021 NVIDIA CORPORATION & AFFILIATES
+ */
+
+#include <linux/acpi.h>
+#include <linux/bitfield.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/gpio/driver.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/irqreturn.h>
+#include <linux/platform_device.h>
+#include <linux/property.h>
+
+#include "mlxbf_gige.h"
+#include "mlxbf_gige_regs.h"
+
+#define MLXBF_GIGE_GPIO_CAUSE_FALL_EN		0x48
+#define MLXBF_GIGE_GPIO_CAUSE_OR_CAUSE_EVTEN0	0x80
+#define MLXBF_GIGE_GPIO_CAUSE_OR_EVTEN0		0x94
+#define MLXBF_GIGE_GPIO_CAUSE_OR_CLRCAUSE	0x98
+
+static void mlxbf_gige_gpio_enable(struct mlxbf_gige *priv)
+{
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&priv->gpio_lock, flags);
+	val = readl(priv->gpio_io + MLXBF_GIGE_GPIO_CAUSE_OR_CLRCAUSE);
+	val |= priv->phy_int_gpio_mask;
+	writel(val, priv->gpio_io + MLXBF_GIGE_GPIO_CAUSE_OR_CLRCAUSE);
+
+	/* The INT_N interrupt level is active low.
+	 * So enable cause fall bit to detect when GPIO
+	 * state goes low.
+	 */
+	val = readl(priv->gpio_io + MLXBF_GIGE_GPIO_CAUSE_FALL_EN);
+	val |= priv->phy_int_gpio_mask;
+	writel(val, priv->gpio_io + MLXBF_GIGE_GPIO_CAUSE_FALL_EN);
+
+	/* Enable PHY interrupt by setting the priority level */
+	val = readl(priv->gpio_io + MLXBF_GIGE_GPIO_CAUSE_OR_EVTEN0);
+	val |= priv->phy_int_gpio_mask;
+	writel(val, priv->gpio_io + MLXBF_GIGE_GPIO_CAUSE_OR_EVTEN0);
+	spin_unlock_irqrestore(&priv->gpio_lock, flags);
+}
+
+static void mlxbf_gige_gpio_disable(struct mlxbf_gige *priv)
+{
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&priv->gpio_lock, flags);
+	val = readl(priv->gpio_io + MLXBF_GIGE_GPIO_CAUSE_OR_EVTEN0);
+	val &= ~priv->phy_int_gpio_mask;
+	writel(val, priv->gpio_io + MLXBF_GIGE_GPIO_CAUSE_OR_EVTEN0);
+	spin_unlock_irqrestore(&priv->gpio_lock, flags);
+}
+
+static irqreturn_t mlxbf_gige_gpio_handler(int irq, void *ptr)
+{
+	struct mlxbf_gige *priv;
+	u32 val;
+
+	priv = ptr;
+
+	/* Check if this interrupt is from PHY device.
+	 * Return if it is not.
+	 */
+	val = readl(priv->gpio_io + MLXBF_GIGE_GPIO_CAUSE_OR_CAUSE_EVTEN0);
+	if (!(val & priv->phy_int_gpio_mask))
+		return IRQ_NONE;
+
+	/* Clear interrupt when done, otherwise, no further interrupt
+	 * will be triggered.
+	 */
+	val = readl(priv->gpio_io + MLXBF_GIGE_GPIO_CAUSE_OR_CLRCAUSE);
+	val |= priv->phy_int_gpio_mask;
+	writel(val, priv->gpio_io + MLXBF_GIGE_GPIO_CAUSE_OR_CLRCAUSE);
+
+	generic_handle_irq(priv->phy_irq);
+
+	return IRQ_HANDLED;
+}
+
+static void mlxbf_gige_gpio_mask(struct irq_data *irqd)
+{
+	struct mlxbf_gige *priv = irq_data_get_irq_chip_data(irqd);
+
+	mlxbf_gige_gpio_disable(priv);
+}
+
+static void mlxbf_gige_gpio_unmask(struct irq_data *irqd)
+{
+	struct mlxbf_gige *priv = irq_data_get_irq_chip_data(irqd);
+
+	mlxbf_gige_gpio_enable(priv);
+}
+
+static struct irq_chip mlxbf_gige_gpio_chip = {
+	.name			= "mlxbf_gige_phy",
+	.irq_mask		= mlxbf_gige_gpio_mask,
+	.irq_unmask		= mlxbf_gige_gpio_unmask,
+};
+
+static int mlxbf_gige_gpio_domain_map(struct irq_domain *d,
+				      unsigned int irq,
+				      irq_hw_number_t hwirq)
+{
+	irq_set_chip_data(irq, d->host_data);
+	irq_set_chip_and_handler(irq, &mlxbf_gige_gpio_chip, handle_simple_irq);
+	irq_set_noprobe(irq);
+
+	return 0;
+}
+
+static const struct irq_domain_ops mlxbf_gige_gpio_domain_ops = {
+	.map    = mlxbf_gige_gpio_domain_map,
+	.xlate	= irq_domain_xlate_twocell,
+};
+
+#ifdef CONFIG_ACPI
+static int mlxbf_gige_gpio_resources(struct acpi_resource *ares,
+				     void *data)
+{
+	struct acpi_resource_gpio *gpio;
+	u32 *phy_int_gpio = data;
+
+	if (ares->type == ACPI_RESOURCE_TYPE_GPIO) {
+		gpio = &ares->data.gpio;
+		*phy_int_gpio = gpio->pin_table[0];
+	}
+
+	return 1;
+}
+#endif
+
+void mlxbf_gige_gpio_free(struct mlxbf_gige *priv)
+{
+	irq_dispose_mapping(priv->phy_irq);
+	irq_domain_remove(priv->irqdomain);
+}
+
+int mlxbf_gige_gpio_init(struct platform_device *pdev,
+			 struct mlxbf_gige *priv)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	u32 phy_int_gpio = 0;
+	int ret;
+
+	LIST_HEAD(resources);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, MLXBF_GIGE_RES_GPIO0);
+	if (!res)
+		return -ENODEV;
+
+	priv->gpio_io = devm_ioremap(dev, res->start, resource_size(res));
+	if (!priv->gpio_io)
+		return -ENOMEM;
+
+#ifdef CONFIG_ACPI
+	ret = acpi_dev_get_resources(ACPI_COMPANION(dev),
+				     &resources, mlxbf_gige_gpio_resources,
+				     &phy_int_gpio);
+	acpi_dev_free_resource_list(&resources);
+	if (ret < 0 || !phy_int_gpio) {
+		dev_err(dev, "Error retrieving the gpio phy pin");
+		return -EINVAL;
+	}
+#endif
+
+	priv->phy_int_gpio_mask = BIT(phy_int_gpio);
+
+	mlxbf_gige_gpio_disable(priv);
+
+	priv->hw_phy_irq = platform_get_irq(pdev, MLXBF_GIGE_PHY_INT_N);
+
+	priv->irqdomain = irq_domain_add_simple(NULL, 1, 0,
+						&mlxbf_gige_gpio_domain_ops,
+						priv);
+	if (!priv->irqdomain) {
+		dev_err(dev, "Failed to add IRQ domain\n");
+		return -ENOMEM;
+	}
+
+	priv->phy_irq = irq_create_mapping(priv->irqdomain, 0);
+	if (!priv->phy_irq) {
+		irq_domain_remove(priv->irqdomain);
+		priv->irqdomain = NULL;
+		dev_err(dev, "Error mapping PHY IRQ\n");
+		return -EINVAL;
+	}
+
+	ret = devm_request_irq(dev, priv->hw_phy_irq, mlxbf_gige_gpio_handler,
+			       IRQF_ONESHOT | IRQF_SHARED, "mlxbf_gige_phy", priv);
+	if (ret) {
+		dev_err(dev, "Failed to request PHY IRQ");
+		mlxbf_gige_gpio_free(priv);
+		return ret;
+	}
+
+	return ret;
+}
diff --git a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_intr.c b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_intr.c
index c63a74d730f8..c38795be04a2 100644
--- a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_intr.c
+++ b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_intr.c
@@ -2,7 +2,7 @@ 
 
 /* Interrupt related logic for Mellanox Gigabit Ethernet driver
  *
- * Copyright (C) 2020-2021 Mellanox Technologies, Ltd. ALL RIGHTS RESERVED.
+ * Copyright (C) 2020-2021 NVIDIA CORPORATION & AFFILIATES
  */
 
 #include <linux/interrupt.h>
@@ -140,4 +140,3 @@  void mlxbf_gige_free_irqs(struct mlxbf_gige *priv)
 	free_irq(priv->rx_irq, priv);
 	free_irq(priv->llu_plu_irq, priv);
 }
-
diff --git a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_main.c b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_main.c
index a17a346baf98..7caa1ca4461f 100644
--- a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_main.c
+++ b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_main.c
@@ -2,14 +2,13 @@ 
 
 /* Gigabit Ethernet driver for Mellanox BlueField SoC
  *
- * Copyright (C) 2020-2021 Mellanox Technologies, Ltd. ALL RIGHTS RESERVED.
+ * Copyright (C) 2020-2021 NVIDIA CORPORATION & AFFILIATES
  */
 
 #include <linux/acpi.h>
 #include <linux/device.h>
 #include <linux/dma-mapping.h>
 #include <linux/etherdevice.h>
-#include <linux/irqdomain.h>
 #include <linux/interrupt.h>
 #include <linux/iopoll.h>
 #include <linux/module.h>
@@ -21,12 +20,12 @@ 
 #include "mlxbf_gige_regs.h"
 
 #define DRV_NAME    "mlxbf_gige"
-#define DRV_VERSION 1.23
+#define DRV_VERSION 1.24
 
 /* This setting defines the version of the ACPI table
  * content that is compatible with this driver version.
  */
-#define MLXBF_GIGE_ACPI_TABLE_VERSION 1
+#define MLXBF_GIGE_ACPI_TABLE_VERSION 2
 
 /* Allocate SKB whose payload pointer aligns with the Bluefield
  * hardware DMA limitation, i.e. DMA operation can't cross
@@ -53,7 +52,7 @@  struct sk_buff *mlxbf_gige_alloc_skb(struct mlxbf_gige *priv,
 	/* Adjust the headroom so that skb->data is naturally aligned to
 	 * a 2KB boundary, which is the maximum packet size supported.
 	 */
-	addr = (u64)skb->data;
+	addr = (long)skb->data;
 	offset = (addr + MLXBF_GIGE_DEFAULT_BUF_SZ - 1) &
 		~(MLXBF_GIGE_DEFAULT_BUF_SZ - 1);
 	offset -= addr;
@@ -170,7 +169,10 @@  static int mlxbf_gige_open(struct net_device *netdev)
 		 MLXBF_GIGE_INT_EN_SW_CONFIG_ERROR |
 		 MLXBF_GIGE_INT_EN_SW_ACCESS_ERROR |
 		 MLXBF_GIGE_INT_EN_RX_RECEIVE_PACKET;
+
+	/* Ensure completion of all initialization before enabling interrupts */
 	mb();
+
 	writeq(int_en, priv->base + MLXBF_GIGE_INT_EN);
 
 	return 0;
@@ -264,20 +266,13 @@  static const struct net_device_ops mlxbf_gige_netdev_ops = {
 
 static void mlxbf_gige_adjust_link(struct net_device *netdev)
 {
-	struct mlxbf_gige *priv = netdev_priv(netdev);
 	struct phy_device *phydev = netdev->phydev;
 
-	if (phydev->link) {
-		priv->rx_pause = phydev->pause;
-		priv->tx_pause = phydev->pause;
-	}
-
 	phy_print_status(phydev);
 }
 
 static int mlxbf_gige_probe(struct platform_device *pdev)
 {
-	unsigned int phy_int_gpio;
 	struct phy_device *phydev;
 	struct net_device *netdev;
 	struct resource *mac_res;
@@ -356,6 +351,13 @@  static int mlxbf_gige_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
+	err = mlxbf_gige_gpio_init(pdev, priv);
+	if (err) {
+		dev_err(&pdev->dev, "PHY IRQ initialization failed\n");
+		mlxbf_gige_mdio_remove(priv);
+		return -ENODEV;
+	}
+
 	priv->base = base;
 	priv->llu_base = llu_base;
 	priv->plu_base = plu_base;
@@ -369,39 +371,29 @@  static int mlxbf_gige_probe(struct platform_device *pdev)
 	err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(64));
 	if (err) {
 		dev_err(&pdev->dev, "DMA configuration failed: 0x%x\n", err);
-		mlxbf_gige_mdio_remove(priv);
-		return err;
+		goto out;
 	}
 
 	priv->error_irq = platform_get_irq(pdev, MLXBF_GIGE_ERROR_INTR_IDX);
 	priv->rx_irq = platform_get_irq(pdev, MLXBF_GIGE_RECEIVE_PKT_INTR_IDX);
 	priv->llu_plu_irq = platform_get_irq(pdev, MLXBF_GIGE_LLU_PLU_INTR_IDX);
 
-	err = device_property_read_u32(&pdev->dev, "phy-int-gpio", &phy_int_gpio);
-	if (err < 0)
-		phy_int_gpio = MLXBF_GIGE_DEFAULT_PHY_INT_GPIO;
-
-	priv->phy_irq = irq_find_mapping(NULL, phy_int_gpio);
-	if (priv->phy_irq == 0) {
-		mlxbf_gige_mdio_remove(priv);
-		return -ENODEV;
-	}
 	phydev = phy_find_first(priv->mdiobus);
 	if (!phydev) {
-		mlxbf_gige_mdio_remove(priv);
-		return -ENODEV;
+		err = -ENODEV;
+		goto out;
 	}
 
 	addr = phydev->mdio.addr;
-	phydev->irq = priv->mdiobus->irq[addr] = priv->phy_irq;
+	priv->mdiobus->irq[addr] = priv->phy_irq;
+	phydev->irq = priv->phy_irq;
 
 	err = phy_connect_direct(netdev, phydev,
 				 mlxbf_gige_adjust_link,
 				 PHY_INTERFACE_MODE_GMII);
 	if (err) {
 		dev_err(&pdev->dev, "Could not attach to PHY\n");
-		mlxbf_gige_mdio_remove(priv);
-		return err;
+		goto out;
 	}
 
 	/* MAC only supports 1000T full duplex mode */
@@ -411,13 +403,11 @@  static int mlxbf_gige_probe(struct platform_device *pdev)
 	phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_10baseT_Full_BIT);
 	phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_10baseT_Half_BIT);
 
-	/* MAC supports symmetric flow control */
-	phy_support_sym_pause(phydev);
-
-	/* Initialise pause frame settings */
-	priv->rx_pause = 0;
-	priv->tx_pause = 0;
-	priv->aneg_pause = AUTONEG_ENABLE;
+	/* Only symmetric pause with flow control enabled is supported so no
+	 * need to negotiate pause.
+	 */
+	linkmode_clear_bit(ETHTOOL_LINK_MODE_Pause_BIT, phydev->advertising);
+	linkmode_clear_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, phydev->advertising);
 
 	/* Display information about attached PHY device */
 	phy_attached_info(phydev);
@@ -426,11 +416,15 @@  static int mlxbf_gige_probe(struct platform_device *pdev)
 	if (err) {
 		dev_err(&pdev->dev, "Failed to register netdev\n");
 		phy_disconnect(phydev);
-		mlxbf_gige_mdio_remove(priv);
-		return err;
+		goto out;
 	}
 
 	return 0;
+
+out:
+	mlxbf_gige_gpio_free(priv);
+	mlxbf_gige_mdio_remove(priv);
+	return err;
 }
 
 static int mlxbf_gige_remove(struct platform_device *pdev)
@@ -439,6 +433,7 @@  static int mlxbf_gige_remove(struct platform_device *pdev)
 
 	unregister_netdev(priv->netdev);
 	phy_disconnect(priv->netdev->phydev);
+	mlxbf_gige_gpio_free(priv);
 	mlxbf_gige_mdio_remove(priv);
 	platform_set_drvdata(pdev, NULL);
 
@@ -453,7 +448,7 @@  static void mlxbf_gige_shutdown(struct platform_device *pdev)
 	mlxbf_gige_clean_port(priv);
 }
 
-static const struct acpi_device_id mlxbf_gige_acpi_match[] = {
+static const struct acpi_device_id __maybe_unused mlxbf_gige_acpi_match[] = {
 	{ "MLNXBF17", 0 },
 	{},
 };
@@ -471,7 +466,6 @@  static struct platform_driver mlxbf_gige_driver = {
 
 module_platform_driver(mlxbf_gige_driver);
 
-MODULE_SOFTDEP("pre: gpio_mlxbf2");
 MODULE_DESCRIPTION("Mellanox BlueField SoC Gigabit Ethernet Driver");
 MODULE_AUTHOR("David Thompson <davthompson@nvidia.com>");
 MODULE_AUTHOR("Asmaa Mnebhi <asmaa@nvidia.com>");
diff --git a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_mdio.c b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_mdio.c
index d1643177ad8a..e32dd34fdcc0 100644
--- a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_mdio.c
+++ b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_mdio.c
@@ -2,7 +2,7 @@ 
 
 /* MDIO support for Mellanox Gigabit Ethernet driver
  *
- * Copyright (C) 2020-2021 Mellanox Technologies, Ltd. ALL RIGHTS RESERVED.
+ * Copyright (C) 2020-2021 NVIDIA CORPORATION & AFFILIATES
  */
 
 #include <linux/acpi.h>
diff --git a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_regs.h b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_regs.h
index d4652f9ce225..5fb33c9294bf 100644
--- a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_regs.h
+++ b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_regs.h
@@ -2,7 +2,7 @@ 
 
 /* Header file for Mellanox BlueField GigE register defines
  *
- * Copyright (C) 2020-2021 Mellanox Technologies, Ltd. ALL RIGHTS RESERVED.
+ * Copyright (C) 2020-2021 NVIDIA CORPORATION & AFFILIATES
  */
 
 #ifndef __MLXBF_GIGE_REGS_H__
diff --git a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_rx.c b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_rx.c
index 12cb5beb29f9..afa3b92a6905 100644
--- a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_rx.c
+++ b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_rx.c
@@ -2,7 +2,7 @@ 
 
 /* Packet receive logic for Mellanox Gigabit Ethernet driver
  *
- * Copyright (C) 2020-2021 Mellanox Technologies, Ltd. ALL RIGHTS RESERVED.
+ * Copyright (C) 2020-2021 NVIDIA CORPORATION & AFFILIATES
  */
 
 #include <linux/etherdevice.h>
@@ -41,6 +41,7 @@  void mlxbf_gige_enable_promisc(struct mlxbf_gige *priv)
 {
 	void __iomem *base = priv->base;
 	u64 control;
+	u64 end_mac;
 
 	/* Enable MAC_ID_RANGE match functionality */
 	control = readq(base + MLXBF_GIGE_CONTROL);
@@ -51,7 +52,8 @@  void mlxbf_gige_enable_promisc(struct mlxbf_gige *priv)
 	writeq(0, base + MLXBF_GIGE_RX_MAC_FILTER_DMAC_RANGE_START);
 
 	/* Set end of destination MAC range check to all FFs */
-	writeq(0xFFFFFFFFFFFF, base + MLXBF_GIGE_RX_MAC_FILTER_DMAC_RANGE_END);
+	end_mac = BCAST_MAC_ADDR;
+	writeq(end_mac, base + MLXBF_GIGE_RX_MAC_FILTER_DMAC_RANGE_END);
 }
 
 void mlxbf_gige_disable_promisc(struct mlxbf_gige *priv)
@@ -266,6 +268,8 @@  static bool mlxbf_gige_rx_packet(struct mlxbf_gige *priv, int *rx_pkts)
 
 	/* Let hardware know we've replenished one buffer */
 	rx_pi++;
+
+	/* Ensure completion of all writes before notifying HW of replenish */
 	wmb();
 	writeq(rx_pi, priv->base + MLXBF_GIGE_RX_WQE_PI);
 
diff --git a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_tx.c b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_tx.c
index 4efa5eff5a98..04982e888c63 100644
--- a/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_tx.c
+++ b/drivers/net/ethernet/mellanox/mlxbf_gige/mlxbf_gige_tx.c
@@ -2,7 +2,7 @@ 
 
 /* Packet transmit logic for Mellanox Gigabit Ethernet driver
  *
- * Copyright (C) 2020-2021 Mellanox Technologies, Ltd. ALL RIGHTS RESERVED.
+ * Copyright (C) 2020-2021 NVIDIA CORPORATION & AFFILIATES
  */
 
 #include <linux/skbuff.h>
@@ -160,6 +160,7 @@  bool mlxbf_gige_handle_tx_complete(struct mlxbf_gige *priv)
 		dev_consume_skb_any(priv->tx_skb[tx_wqe_index]);
 		priv->tx_skb[tx_wqe_index] = NULL;
 
+		/* Ensure completion of updates across all cores */
 		mb();
 	}
 
@@ -199,7 +200,7 @@  netdev_tx_t mlxbf_gige_start_xmit(struct sk_buff *skb,
 	u64 word2;
 
 	/* If needed, linearize TX SKB as hardware DMA expects this */
-	if ((skb->len > MLXBF_GIGE_DEFAULT_BUF_SZ) || skb_linearize(skb)) {
+	if (skb->len > MLXBF_GIGE_DEFAULT_BUF_SZ || skb_linearize(skb)) {
 		dev_kfree_skb(skb);
 		netdev->stats.tx_dropped++;
 		return NETDEV_TX_OK;