From 16502bfa7c969cff366f92de67c048d2b0c626b8 Mon Sep 17 00:00:00 2001
From: Claudiu Beznea <claudiu.beznea@microchip.com>
Date: Mon, 7 Sep 2020 17:46:48 +0300
Subject: [PATCH] clk: at91: clk-system: add driver compatible with ccf

Add clk-system driver compatible with common clock framework.

Signed-off-by: Claudiu Beznea <claudiu.beznea@microchip.com>
---
 drivers/clk/at91/Makefile     |   2 +-
 drivers/clk/at91/clk-system.c | 112 ++++++++++++++++++++++++++++++++++
 drivers/clk/at91/pmc.h        |   3 +
 3 files changed, 116 insertions(+), 1 deletion(-)
 create mode 100644 drivers/clk/at91/clk-system.c

diff --git a/drivers/clk/at91/Makefile b/drivers/clk/at91/Makefile
index 8951052eb0..1d59531e0c 100644
--- a/drivers/clk/at91/Makefile
+++ b/drivers/clk/at91/Makefile
@@ -3,7 +3,7 @@
 #
 
 ifdef CONFIG_CLK_CCF
-obj-y += pmc.o sckc.o clk-main.o clk-master.o clk-programmable.o
+obj-y += pmc.o sckc.o clk-main.o clk-master.o clk-programmable.o clk-system.o
 obj-$(CONFIG_AT91_UTMI)		+= clk-utmi.o
 obj-$(CONFIG_AT91_SAM9X60_PLL)	+= clk-sam9x60-pll.o
 else
diff --git a/drivers/clk/at91/clk-system.c b/drivers/clk/at91/clk-system.c
new file mode 100644
index 0000000000..82f79e74a1
--- /dev/null
+++ b/drivers/clk/at91/clk-system.c
@@ -0,0 +1,112 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * System clock support for AT91 architectures.
+ *
+ * Copyright (C) Microchip Technology Inc. and its subsidiaries
+ *
+ * Author: Claudiu Beznea <claudiu.beznea@microchip.com>
+ *
+ * Based on drivers/clk/at91/clk-system.c from Linux.
+ */
+#include <asm/processor.h>
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm.h>
+#include <linux/io.h>
+#include <linux/clk-provider.h>
+#include <linux/clk/at91_pmc.h>
+
+#include "pmc.h"
+
+#define UBOOT_DM_CLK_AT91_SYSTEM		"at91-system-clk"
+
+#define SYSTEM_MAX_ID		31
+
+struct clk_system {
+	void __iomem *base;
+	struct clk clk;
+	u8 id;
+};
+
+#define to_clk_system(_c) container_of(_c, struct clk_system, clk)
+
+static inline int is_pck(int id)
+{
+	return (id >= 8) && (id <= 15);
+}
+
+static inline bool clk_system_ready(void __iomem *base, int id)
+{
+	unsigned int status;
+
+	pmc_read(base, AT91_PMC_SR, &status);
+
+	return !!(status & (1 << id));
+}
+
+static int clk_system_enable(struct clk *clk)
+{
+	struct clk_system *sys = to_clk_system(clk);
+
+	pmc_write(sys->base, AT91_PMC_SCER, 1 << sys->id);
+
+	if (!is_pck(sys->id))
+		return 0;
+
+	while (!clk_system_ready(sys->base, sys->id)) {
+		debug("waiting for pck%u\n", sys->id);
+		cpu_relax();
+	}
+
+	return 0;
+}
+
+static int clk_system_disable(struct clk *clk)
+{
+	struct clk_system *sys = to_clk_system(clk);
+
+	pmc_write(sys->base, AT91_PMC_SCDR, 1 << sys->id);
+
+	return 0;
+}
+
+static const struct clk_ops system_ops = {
+	.enable = clk_system_enable,
+	.disable = clk_system_disable,
+	.get_rate = clk_generic_get_rate,
+};
+
+struct clk *at91_clk_register_system(void __iomem *base, const char *name,
+				     const char *parent_name, u8 id)
+{
+	struct clk_system *sys;
+	struct clk *clk;
+	int ret;
+
+	if (!base || !name || !parent_name || id > SYSTEM_MAX_ID)
+		return ERR_PTR(-EINVAL);
+
+	sys = kzalloc(sizeof(*sys), GFP_KERNEL);
+	if (!sys)
+		return ERR_PTR(-ENOMEM);
+
+	sys->id = id;
+	sys->base = base;
+
+	clk = &sys->clk;
+	clk->flags = CLK_GET_RATE_NOCACHE;
+	ret = clk_register(clk, UBOOT_DM_CLK_AT91_SYSTEM, name, parent_name);
+	if (ret) {
+		kfree(sys);
+		clk = ERR_PTR(ret);
+	}
+
+	return clk;
+}
+
+U_BOOT_DRIVER(at91_system_clk) = {
+	.name = UBOOT_DM_CLK_AT91_SYSTEM,
+	.id = UCLASS_CLK,
+	.ops = &system_ops,
+	.flags = DM_FLAG_PRE_RELOC,
+};
diff --git a/drivers/clk/at91/pmc.h b/drivers/clk/at91/pmc.h
index a443b65257..c372f39fc9 100644
--- a/drivers/clk/at91/pmc.h
+++ b/drivers/clk/at91/pmc.h
@@ -108,6 +108,9 @@ at91_clk_register_programmable(void __iomem *base, const char *name,
 			const char * const *parent_names, u8 num_parents, u8 id,
 			const struct clk_programmable_layout *layout,
 			const u32 *clk_mux_table, const u32 *mux_table);
+struct clk *
+at91_clk_register_system(void __iomem *base, const char *name,
+			const char *parent_name, u8 id);
 
 int at91_clk_mux_val_to_index(const u32 *table, u32 num_parents, u32 val);
 int at91_clk_mux_index_to_val(const u32 *table, u32 num_parents, u32 index);
-- 
2.39.5