[PATCH 2/2] clk: versal: Enable clock driver for Versal Gen 2

[Michal Simek]

Versal Gen 2 is using enhancement SMC format but in near future SCMI client
should be used. This patch is just bridging this gap till SCMI server is
fully tested.

Signed-off-by: Michal Simek <michal.simek at amd.com>
---

 configs/amd_versal2_virt_defconfig |  1 +
 drivers/clk/Kconfig                |  2 +-
 drivers/clk/clk_versal.c           | 20 ++++++++++++++++++++
 3 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/configs/amd_versal2_virt_defconfig b/configs/amd_versal2_virt_defconfig
index caf4aefe898d..4f0c4ba5c967 100644
--- a/configs/amd_versal2_virt_defconfig
+++ b/configs/amd_versal2_virt_defconfig
@@ -75,6 +75,7 @@ CONFIG_NET_RANDOM_ETHADDR=y
 CONFIG_SIMPLE_PM_BUS=y
 CONFIG_CLK_CCF=y
 CONFIG_CLK_SCMI=y
+CONFIG_CLK_VERSAL=y
 CONFIG_DFU_RAM=y
 CONFIG_ARM_FFA_TRANSPORT=y
 CONFIG_FPGA_XILINX=y
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index b884a02bdeba..85cc472b4cb9 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -224,7 +224,7 @@ config CLK_VERSACLOCK
 
 config CLK_VERSAL
 	bool "Enable clock driver support for Versal"
-	depends on (ARCH_VERSAL || ARCH_VERSAL_NET)
+	depends on (ARCH_VERSAL || ARCH_VERSAL_NET || ARCH_VERSAL2)
 	depends on ZYNQMP_FIRMWARE
 	help
 	  This clock driver adds support for clock realted settings for
diff --git a/drivers/clk/clk_versal.c b/drivers/clk/clk_versal.c
index 4a498e22f967..78a2410ca21c 100644
--- a/drivers/clk/clk_versal.c
+++ b/drivers/clk/clk_versal.c
@@ -136,6 +136,25 @@ static int versal_pm_query_legacy(struct versal_pm_query_data qdata,
 	return qdata.qid == PM_QID_CLOCK_GET_NAME ? 0 : ret;
 }
 
+static int versal_pm_query_enhanced(struct versal_pm_query_data qdata,
+				    u32 *ret_payload)
+{
+	int ret;
+
+	ret = smc_call_handler(PM_QUERY_DATA, qdata.qid, qdata.arg1, qdata.arg2,
+			       qdata.arg3, 0, 0, ret_payload);
+
+	if (qdata.qid == PM_QID_CLOCK_GET_NAME) {
+		ret_payload[0] = ret_payload[1];
+		ret_payload[1] = ret_payload[2];
+		ret_payload[2] = ret_payload[3];
+		ret_payload[3] = ret_payload[4];
+		ret_payload[4] = 0;
+	}
+
+	return ret;
+}
+
 static inline int versal_is_valid_clock(u32 clk_id)
 {
 	if (clk_id >= clock_max_idx)
@@ -794,6 +813,7 @@ static struct clk_ops versal_clk_ops = {
 
 static const struct udevice_id versal_clk_ids[] = {
 	{ .compatible = "xlnx,versal-clk", .data = (ulong)versal_pm_query_legacy },
+	{ .compatible = "xlnx,versal2-clk", .data = (ulong)versal_pm_query_enhanced },
 	{ }
 };
 
-- 
2.43.0

base-commit: f109829793900e57558d98ed22caf80c1a72b232