[PATCH v2 3/5] clk: stm32: Add STM32MP25 SYSCFG clock driver

[Gabriel Fernandez]

On STM32MP23x and STM32MP25x, the LTDC pixel clock is selected in the
SYSCFG DISPLAYCLKCR register among the DSI PHY byte lane clock, the LVDS
PHY pixel clock (reset value) and the LTDC kernel clock, the latter
being the one needed by the RGB (DPI) output.

No driver registers this clock, so the selection can't be changed from
its reset value and a display connected to the LTDC RGB output can't
work.

Add a driver registering this clock as a mux. The SYSCFG node is bound
to the STM32MP2 SYSCFG driver, which populates its sub-nodes, so the
driver is probed from the "st,stm32mp25-syscfg-clk" clock-controller
sub-node and uses the regmap of the parent SYSCFG node. The mux parents
are taken from the "clocks" property of the sub-node, and the clock is
exposed through a onecell provider so that the other SYSCFG clocks can
be added later.

Co-developed-by: Dario Binacchi <dario.binacchi@xxxxxxxxxxxxxxxxxxxx>
Signed-off-by: Dario Binacchi <dario.binacchi@xxxxxxxxxxxxxxxxxxxx>
Signed-off-by: Gabriel Fernandez <gabriel.fernandez@xxxxxxxxxxx>
---
 drivers/clk/stm32/Kconfig                |  11 +++
 drivers/clk/stm32/Makefile               |   1 +
 drivers/clk/stm32/clk-stm32mp25-syscfg.c | 134 +++++++++++++++++++++++++++++++
 3 files changed, 146 insertions(+)

diff --git a/drivers/clk/stm32/Kconfig b/drivers/clk/stm32/Kconfig
index 5dbd75cde657..a298983f5511 100644
--- a/drivers/clk/stm32/Kconfig
+++ b/drivers/clk/stm32/Kconfig
@@ -39,5 +39,16 @@ config COMMON_CLK_STM32MP257
 	help
 	  Support for stm32mp25x SoC family clocks.
 
+config COMMON_CLK_STM32MP25_SYSCFG
+	tristate "Clock driver for stm32mp25x SYSCFG clocks"
+	depends on COMMON_CLK_STM32MP257
+	select MFD_SYSCON
+	default COMMON_CLK_STM32MP257
+	help
+	  Support for the clocks provided by the stm32mp25x SYSCFG, i.e.
+	  the LTDC pixel clock source selection between the LTDC kernel
+	  clock (RGB output), the LVDS PHY pixel clock and the DSI byte
+	  lane clock.
+
 endif
 
diff --git a/drivers/clk/stm32/Makefile b/drivers/clk/stm32/Makefile
index e04727b59449..8088d172651f 100644
--- a/drivers/clk/stm32/Makefile
+++ b/drivers/clk/stm32/Makefile
@@ -2,3 +2,4 @@ obj-$(CONFIG_COMMON_CLK_STM32MP135)	+= clk-stm32mp13.o clk-stm32-core.o reset-st
 obj-$(CONFIG_COMMON_CLK_STM32MP157)	+= clk-stm32mp1.o reset-stm32.o
 obj-$(CONFIG_COMMON_CLK_STM32MP215)	+= clk-stm32mp21.o clk-stm32-core.o reset-stm32.o
 obj-$(CONFIG_COMMON_CLK_STM32MP257)	+= clk-stm32mp25.o clk-stm32-core.o reset-stm32.o
+obj-$(CONFIG_COMMON_CLK_STM32MP25_SYSCFG)	+= clk-stm32mp25-syscfg.o
diff --git a/drivers/clk/stm32/clk-stm32mp25-syscfg.c b/drivers/clk/stm32/clk-stm32mp25-syscfg.c
new file mode 100644
index 000000000000..69b9a7592838
--- /dev/null
+++ b/drivers/clk/stm32/clk-stm32mp25-syscfg.c
@@ -0,0 +1,134 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) STMicroelectronics 2023 - All Rights Reserved
+ * Author: Gabriel Fernandez <gabriel.fernandez@xxxxxxxxxxx> for STMicroelectronics.
+ */
+
+#include <linux/bitfield.h>
+#include <linux/bits.h>
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/mfd/syscon.h>
+#include <linux/mod_devicetable.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include <dt-bindings/clock/st,stm32mp25-syscfg-clk.h>
+
+#define SYSCFG_DISPLAYCLKCR		0x5000
+#define DISPLAYCLKCR_DPIVSLVDS		GENMASK(1, 0)
+
+#define STM32MP25_SYSCFG_NUM_CLKS	(CK_SYSCFG_PIX_LTDC + 1)
+
+/*
+ * The LTDC pixel clock source is selected in SYSCFG_DISPLAYCLKCR among
+ * the DSI byte lane clock, the LVDS PHY pixel clock (reset value) and
+ * the LTDC kernel clock, which is used with the RGB (DPI) output.
+ *
+ * The parents are listed in the DPIVSLVDS field order, while .index refers
+ * to the "clocks" property of the node, ordered "ltdc", "lvds", "dsi" so
+ * that the inputs missing on a SoC can be omitted at the end of the list.
+ */
+static const struct clk_parent_data clk_pix_ltdc_parents[] = {
+	{ .index = 2 },	/* dsi */
+	{ .index = 1 },	/* lvds */
+	{ .index = 0 },	/* ltdc */
+};
+
+struct stm32mp25_syscfg_mux {
+	struct clk_hw hw;
+	struct regmap *regmap;
+};
+
+static inline struct stm32mp25_syscfg_mux *to_syscfg_mux(struct clk_hw *hw)
+{
+	return container_of(hw, struct stm32mp25_syscfg_mux, hw);
+}
+
+static u8 stm32mp25_syscfg_mux_get_parent(struct clk_hw *hw)
+{
+	struct stm32mp25_syscfg_mux *mux = to_syscfg_mux(hw);
+	unsigned int val;
+
+	regmap_read(mux->regmap, SYSCFG_DISPLAYCLKCR, &val);
+
+	return FIELD_GET(DISPLAYCLKCR_DPIVSLVDS, val);
+}
+
+static int stm32mp25_syscfg_mux_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct stm32mp25_syscfg_mux *mux = to_syscfg_mux(hw);
+
+	return regmap_update_bits(mux->regmap, SYSCFG_DISPLAYCLKCR,
+				  DISPLAYCLKCR_DPIVSLVDS,
+				  FIELD_PREP(DISPLAYCLKCR_DPIVSLVDS, index));
+}
+
+static const struct clk_ops stm32mp25_syscfg_mux_ops = {
+	.get_parent = stm32mp25_syscfg_mux_get_parent,
+	.set_parent = stm32mp25_syscfg_mux_set_parent,
+	.determine_rate = clk_hw_determine_rate_no_reparent,
+};
+
+static int stm32mp25_syscfg_clk_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct clk_hw_onecell_data *clk_data;
+	struct clk_init_data init = { };
+	struct stm32mp25_syscfg_mux *mux;
+	int ret;
+
+	clk_data = devm_kzalloc_flex(dev, *clk_data, hws,
+				     STM32MP25_SYSCFG_NUM_CLKS);
+	if (!clk_data)
+		return -ENOMEM;
+
+	clk_data->num = STM32MP25_SYSCFG_NUM_CLKS;
+
+	mux = devm_kzalloc_obj(dev, *mux);
+	if (!mux)
+		return -ENOMEM;
+
+	/* The mux is a sub-node of the SYSCFG, whose regmap it shares */
+	mux->regmap = syscon_node_to_regmap(dev->parent->of_node);
+	if (IS_ERR(mux->regmap))
+		return dev_err_probe(dev, PTR_ERR(mux->regmap),
+				     "failed to get the SYSCFG regmap\n");
+
+	init.name = "clk_pix_ltdc";
+	init.ops = &stm32mp25_syscfg_mux_ops;
+	init.parent_data = clk_pix_ltdc_parents;
+	init.num_parents = ARRAY_SIZE(clk_pix_ltdc_parents);
+	init.flags = CLK_SET_RATE_PARENT | CLK_SET_RATE_NO_REPARENT;
+	mux->hw.init = &init;
+
+	ret = devm_clk_hw_register(dev, &mux->hw);
+	if (ret)
+		return dev_err_probe(dev, ret, "failed to register %s\n",
+				     init.name);
+
+	clk_data->hws[CK_SYSCFG_PIX_LTDC] = &mux->hw;
+
+	return devm_of_clk_add_hw_provider(dev, of_clk_hw_onecell_get, clk_data);
+}
+
+static const struct of_device_id stm32mp25_syscfg_clk_of_match[] = {
+	{ .compatible = "st,stm32mp25-syscfg-clk" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, stm32mp25_syscfg_clk_of_match);
+
+static struct platform_driver stm32mp25_syscfg_clk_driver = {
+	.probe = stm32mp25_syscfg_clk_probe,
+	.driver = {
+		.name = "stm32mp25-syscfg-clk",
+		.of_match_table = stm32mp25_syscfg_clk_of_match,
+	},
+};
+module_platform_driver(stm32mp25_syscfg_clk_driver);
+
+MODULE_AUTHOR("Gabriel Fernandez <gabriel.fernandez@xxxxxxxxxxx>");
+MODULE_DESCRIPTION("STM32MP25 SYSCFG clock driver");
+MODULE_LICENSE("GPL");

-- 
2.43.0