VisionFive2 Linux kernel

diff --git a/drivers/pinctrl/pinctrl-starfive.c b/drivers/pinctrl/pinctrl-starfive.c
index f2a4792feb6e..54fe85963619 100644
--- a/drivers/pinctrl/pinctrl-starfive.c
+++ b/drivers/pinctrl/pinctrl-starfive.c
@@ -168,6 +168,10 @@
 #define PAD_DRIVE_STRENGTH_MASK		0x007U
 #define PAD_DRIVE_STRENGTH_POS		0
 
+static bool keepmux;
+module_param(keepmux, bool, 0644);
+MODULE_PARM_DESC(keepmux, "Keep pinmux settings from previous boot stage");
+
 struct starfive_pinctrl {
 	struct gpio_chip gc;
 	struct pinctrl_gpio_range gpios;
@@ -1279,6 +1283,67 @@ static int starfive_gpio_init_hw(struct gpio_chip *gc)
 	return 0;
 }
 
+#define MAX_GPI (GPI_USB_OVER_CURRENT + 1)
+static void starfive_pinmux_reset(struct starfive_pinctrl *sfp)
+{
+	static const DECLARE_BITMAP(defaults, MAX_GPI) = {
+		BIT_MASK(GPI_I2C0_PAD_SCK_IN) |
+		BIT_MASK(GPI_I2C0_PAD_SDA_IN) |
+		BIT_MASK(GPI_I2C1_PAD_SCK_IN) |
+		BIT_MASK(GPI_I2C1_PAD_SDA_IN) |
+		BIT_MASK(GPI_I2C2_PAD_SCK_IN) |
+		BIT_MASK(GPI_I2C2_PAD_SDA_IN) |
+		BIT_MASK(GPI_I2C3_PAD_SCK_IN) |
+		BIT_MASK(GPI_I2C3_PAD_SDA_IN) |
+		BIT_MASK(GPI_SDIO0_PAD_CARD_DETECT_N) |
+
+		BIT_MASK(GPI_SDIO1_PAD_CARD_DETECT_N) |
+		BIT_MASK(GPI_SPI0_PAD_SS_IN_N) |
+		BIT_MASK(GPI_SPI1_PAD_SS_IN_N) |
+		BIT_MASK(GPI_SPI2_PAD_SS_IN_N) |
+		BIT_MASK(GPI_SPI2AHB_PAD_SS_N) |
+		BIT_MASK(GPI_SPI3_PAD_SS_IN_N),
+
+		BIT_MASK(GPI_UART0_PAD_SIN) |
+		BIT_MASK(GPI_UART1_PAD_SIN) |
+		BIT_MASK(GPI_UART2_PAD_SIN) |
+		BIT_MASK(GPI_UART3_PAD_SIN) |
+		BIT_MASK(GPI_USB_OVER_CURRENT)
+	};
+	DECLARE_BITMAP(keep, MAX_GPIO) = { 0 };
+	const __be32 *list;
+	int size = 0;
+	int i;
+
+	list = of_get_property(starfive_dev(sfp)->of_node, "starfive,keep-gpiomux", &size);
+	for (i = 0; i < size; i += sizeof(*list)) {
+		u32 gpio = be32_to_cpu(*list++);
+
+		if (gpio < MAX_GPIO)
+			set_bit(gpio, keep);
+	}
+
+	for (i = 0; i < MAX_GPIO; i++) {
+		if (test_bit(i, keep))
+			continue;
+
+		writel_relaxed(GPO_DISABLE,
+			       sfp->base + GPIO_N_DOEN_CFG + 8 * i);
+		writel_relaxed(GPO_LOW,
+			       sfp->base + GPIO_N_DOUT_CFG + 8 * i);
+	}
+
+	for (i = 0; i < MAX_GPI; i++) {
+		void __iomem *reg = sfp->base + GPIO_IN_OFFSET + 4 * i;
+		u32 din = readl_relaxed(reg);
+
+		if (din >= 2 && din < (MAX_GPIO + 2) && test_bit(din - 2, keep))
+			continue;
+
+		writel_relaxed(test_bit(i, defaults), reg);
+	}
+}
+
 static int __init starfive_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -1312,6 +1377,9 @@ static int __init starfive_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	if (!keepmux)
+		starfive_pinmux_reset(sfp);
+
 	if (!of_property_read_u32(dev->of_node, "starfive,signal-group", &value)) {
 		if (value <= 6)
 			writel(value, sfp->padctl + IO_PADSHARE_SEL);