[v2,3/9] arm: twr-k70f120m: clock driver for Kinetis SoC
diff mbox

Message ID alpine.LNX.2.00.1507062248290.9686@localhost.localdomain
State New
Headers show

Commit Message

Paul Osmialowski July 6, 2015, 8:57 p.m. UTC
Hi Guys,

Let me share with you one more approach. I moved clocks back to 
sub-devices, so sharing the same resources (registers) is more obvious 
again. I like it better than previous approach. Can you look at this, 
please?

On Sat, 4 Jul 2015, Paul Osmialowski wrote:

> Hi Arnd,
>
> I'm attaching excerpt from Kinetis reference manual that may make situation 
> clearer.
>
> These MCG and SIM registers are used only to determine configuration (clock 
> fixed rates and clock signal origins) at run time.
>
> Namely, the real MCGOUTCLK source (in the middle) which is the parent for 
> core clock (CCLK) and peripheral clock (PCLK) is determined at run time by 
> reading MCG registers, let me quote commit message from Emcraft git repo:
>
>     * Determine in run-time what oscillator module (OSC0 or OSC1) is used
>     as clock source for the main PLL.
>     * When OSC1 is selected, assume its frequency to be 12 MHz on all
>     boards (there is a 12 MHz oscillator on XTAL1/EXTAL1 on K70-SOM and
>     TWR-K70F120M boards).
>
> In my .dts I'm trying to possibly follow real clock hierarchy, but to go 
> anywhere behind MCGOUTCLK would require ability to rewrite .dtb e.g. by 
> U-boot. But that's too demanding for any potential users of this BSP. So 
> let's asume that MCGOUTCLK is the root clock and a parent for CCLK and PCLK.
>
> In my most recent version I added OSC0ERCLK explicitly as one more root 
> clock, since it is also used directly (through CG reg. 1 bit 0) by Freescale 
> fec network device whose in-tree driver I'm trying to make usable for 
> Kinetis.
>
> On Sat, 4 Jul 2015, Arnd Bergmann wrote:
>
>>  On Friday 03 July 2015 00:08:27 Thomas Gleixner wrote:
>> >  On Thu, 2 Jul 2015, Paul Osmialowski wrote:
>> > >  On Thu, 2 Jul 2015, Arnd Bergmann wrote:
>> > > 
>> > > >  I wonder if you could move out the fixed rate clocks into their own
>> > > >  nodes. Are they actually controlled by the same block? If they are
>> > > >  just fixed, you can use the normal binding for fixed rate clocks
>> > > >  and only describe the clocks that are related to the driver.
>> > > 
>> > >  In my view having these clocks grouped together looks more convincing. 
>> > >  After
>> > >  all, they all share the same I/O regs in order to read configuration.
>> > 
>> >  The fact that they share a register is not making them a group. That's
>> >  just a HW design decision and you need to deal with that by protecting
>> >  the register access, but not by trying to group them artificially at
>> >  the functional level.
>>
>>  I'd disagree with that: The clock controller is the device that owns the
>>  registers and that should be one node in DT, as Paul's first version does.
>>
>>  The part I'm still struggling with is understanding how the fixed-rate
>>  clocks are controlled through those registers. If they are indeed
>>  configured
>>  through the registers, the name is probably wrong and should be changed
>>  to whatever kind of non-fixed clock this is.
>>
>>   Arnd
>> 
>

Patch
diff mbox

From 273fdc022a44e2c4ac94a8bc070dd42a8f299215 Mon Sep 17 00:00:00 2001
From: Paul Osmialowski <pawelo@king.net.pl>
Date: Mon, 29 Jun 2015 20:58:55 +0200
Subject: [PATCH 3/9] arm: twr-k70f120m: clock driver for Kinetis SoC

Based on K70P256M150SF3RM.pdf K70 Sub-Family Reference Manual, Rev. 3.

Signed-off-by: Paul Osmialowski <pawelo@king.net.pl>
---
 .../devicetree/bindings/clock/kinetis-clock.txt    |  82 ++++
 arch/arm/boot/dts/kinetis.dtsi                     |  43 ++
 drivers/clk/Makefile                               |   1 +
 drivers/clk/clk-kinetis.c                          | 532 +++++++++++++++++++++
 4 files changed, 658 insertions(+)
 create mode 100644 Documentation/devicetree/bindings/clock/kinetis-clock.txt
 create mode 100644 drivers/clk/clk-kinetis.c

diff --git a/Documentation/devicetree/bindings/clock/kinetis-clock.txt b/Documentation/devicetree/bindings/clock/kinetis-clock.txt
new file mode 100644
index 0000000..d7c4ebf
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/kinetis-clock.txt
@@ -0,0 +1,82 @@ 
+* Clock bindings for Freescale Kinetis SoC
+
+Required properties:
+- compatible: Should be "fsl,kinetis-cmu".
+- reg: Two address ranges:
+	- "mcg" for the Multipurpose Clock Genetator (MCG) register set.
+	- "sim" for System Integration Module (SIM) register set.
+- reg-names: Should name the address ranges ("mcg" and "sim" respectively).
+
+A set of clock sub-devices should be also provided. Each sub-device should be
+a fixed-rate clock or a clock gate.
+
+Required properties for sub-devices:
+- clocks: single element list of parent clocks (only for non-root clocks).
+- #clock-cells: For fixed rate clocks must be <0>,
+		for clock gates must be <2> (see below).
+
+For clock gate addresses see K70 Sub-Family Reference Manual, Rev. 3 pg. 341
+and later. Notice that addresses are zero-based, so SIM_SCGC1 has address 0,
+SIM_SCGC2 has address 1 and so on. The second address component is the bit
+index.
+
+Notice that (non-gate) clock device names must be known to Kinetis CMU drivers.
+
+Currently known names are:
+
+o fixed-rate-mcgout (root)
+o fixed-rate-osc0er (root)
+o fixed-rate-cclk (fixed-rate-mcgout child)
+o fixed-rate-pclk (fixed-rate-mcgout child)
+
+Example:
+
+cmu@40064000 {
+	compatible = "fsl,kinetis-cmu";
+	reg = <0x40064000 0x14>, <0x40047000 0x1100>;
+	reg-names = "mcg", "sim";
+
+	mcg_outclk: fixed-rate-mcgout {
+		#clock-cells = <0>;
+	};
+
+	mcg_cclk: fixed-rate-cclk {
+		clocks = <&mcg_outclk>;
+		#clock-cells = <0>;
+	};
+
+	mcg_pclk: fixed-rate-pclk {
+		clocks = <&mcg_outclk>;
+		#clock-cells = <0>;
+	};
+
+	mcg_cclk_gate: cclk-gate {
+		clocks = <&mcg_cclk>;
+		#clock-cells = <2>;
+	};
+
+	mcg_pclk_gate: pclk-gate {
+		clocks = <&mcg_pclk>;
+		#clock-cells = <2>;
+	};
+
+	osc0_erclk: fixed-rate-osc0er {
+		#clock-cells = <0>;
+	};
+
+	osc0_erclk_gate: osc0-gate {
+		clocks = <&osc0_erclk>;
+		#clock-cells = <2>;
+	};
+};
+
+uart1: serial@4006b000 {
+	compatible = "fsl,kinetis-lpuart";
+	reg = <0x4006b000 0x1000>;
+	interrupts = <47>, <48>;
+	interrupt-names = "uart-stat", "uart-err";
+	clocks = <&mcg_cclk_gate 3 11>;
+	clock-names = "ipg";
+	dmas = <&edma 0 4>;
+	dma-names = "rx";
+};
diff --git a/arch/arm/boot/dts/kinetis.dtsi b/arch/arm/boot/dts/kinetis.dtsi
index 93d2a8a..ad58168 100644
--- a/arch/arm/boot/dts/kinetis.dtsi
+++ b/arch/arm/boot/dts/kinetis.dtsi
@@ -3,3 +3,46 @@ 
  *
  */
 #include "armv7-m.dtsi"
+
+/ {
+	soc {
+		cmu@40064000 {
+			compatible = "fsl,kinetis-cmu";
+			reg = <0x40064000 0x14>, <0x40047000 0x1100>;
+			reg-names = "mcg", "sim";
+
+			mcg_outclk: fixed-rate-mcgout {
+				#clock-cells = <0>;
+			};
+
+			mcg_cclk: fixed-rate-cclk {
+				clocks = <&mcg_outclk>;
+				#clock-cells = <0>;
+			};
+
+			mcg_pclk: fixed-rate-pclk {
+				clocks = <&mcg_outclk>;
+				#clock-cells = <0>;
+			};
+
+			mcg_cclk_gate: cclk-gate {
+				clocks = <&mcg_cclk>;
+				#clock-cells = <2>;
+			};
+
+			mcg_pclk_gate: pclk-gate {
+				clocks = <&mcg_pclk>;
+				#clock-cells = <2>;
+			};
+
+			osc0_erclk: fixed-rate-osc0er {
+				#clock-cells = <0>;
+			};
+
+			osc0_erclk_gate: osc0-gate {
+				clocks = <&osc0_erclk>;
+				#clock-cells = <2>;
+			};
+		};
+	};
+};
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index c4cf075..0d48167 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -24,6 +24,7 @@  obj-$(CONFIG_COMMON_CLK_CDCE706)	+= clk-cdce706.o
 obj-$(CONFIG_ARCH_CLPS711X)		+= clk-clps711x.o
 obj-$(CONFIG_ARCH_EFM32)		+= clk-efm32gg.o
 obj-$(CONFIG_ARCH_HIGHBANK)		+= clk-highbank.o
+obj-$(CONFIG_ARCH_KINETIS)		+= clk-kinetis.o
 obj-$(CONFIG_MACH_LOONGSON32)		+= clk-ls1x.o
 obj-$(CONFIG_COMMON_CLK_MAX_GEN)	+= clk-max-gen.o
 obj-$(CONFIG_COMMON_CLK_MAX77686)	+= clk-max77686.o
diff --git a/drivers/clk/clk-kinetis.c b/drivers/clk/clk-kinetis.c
new file mode 100644
index 0000000..d23153d
--- /dev/null
+++ b/drivers/clk/clk-kinetis.c
@@ -0,0 +1,532 @@ 
+/*
+ * clk-kinetis.c - Clock driver for Kinetis K70 MCG
+ *
+ * Based on legacy pre-OF code by Alexander Potashev <aspotashev@emcraft.com>
+ *
+ * Copyright (C) 2015 Paul Osmialowski <pawelo@king.net.pl>
+ *
+ * This program is free software; you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License version 2 as published by the
+ * Free Software Foundation.
+ */
+
+#include <linux/io.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+
+enum kinetis_clk_ids {
+	KINETIS_CLK_MCGOUT = 0,
+	KINETIS_CLK_OSC0ER,
+	KINETIS_CLK_CCLK,
+	KINETIS_CLK_PCLK,
+	KINETIS_CLK_NUM
+};
+
+static const struct {
+	enum kinetis_clk_ids	id;
+	const char		*name;
+} kinetis_clks[KINETIS_CLK_NUM] = {
+	{ .id = KINETIS_CLK_MCGOUT,	.name = "fixed-rate-mcgout",	},
+	{ .id = KINETIS_CLK_OSC0ER,	.name = "fixed-rate-osc0er",	},
+	{ .id = KINETIS_CLK_CCLK,	.name = "fixed-rate-cclk",	},
+	{ .id = KINETIS_CLK_PCLK,	.name = "fixed-rate-pclk",	},
+};
+
+/*
+ * Frequencies on OSC0 (EXTAL0/XTAL0) and OSC1 (EXTAL1/XTAL1)
+ *
+ * These frequencies should be set to the same values as in U-Boot.
+ */
+#define KINETIS_OSC0_RATE	50000000	/* 50 MHz */
+#define KINETIS_OSC1_RATE	12000000	/* 12 MHz */
+
+#define KINETIS_SIM_CG_NUMREGS	7
+
+/*
+ * System Integration Module (SIM) register map
+ *
+ * This map actually covers two hardware modules:
+ *     1. SIM low-power logic, at 0x40047000
+ *     2. System integration module (SIM), at 0x40048000
+ */
+struct kinetis_sim_regs {
+	u32 sopt1;	/* System Options Register 1 */
+	u32 rsv0[1024];
+	u32 sopt2;	/* System Options Register 2 */
+	u32 rsv1;
+	u32 sopt4;	/* System Options Register 4 */
+	u32 sopt5;	/* System Options Register 5 */
+	u32 sopt6;	/* System Options Register 6 */
+	u32 sopt7;	/* System Options Register 7 */
+	u32 rsv2[2];
+	u32 sdid;	/* System Device Identification Register */
+	u32 scgc[KINETIS_SIM_CG_NUMREGS];	/* Clock Gating Regs 1...7 */
+	u32 clkdiv1;	/* System Clock Divider Register 1 */
+	u32 clkdiv2;	/* System Clock Divider Register 2 */
+	u32 fcfg1;	/* Flash Configuration Register 1 */
+	u32 fcfg2;	/* Flash Configuration Register 2 */
+	u32 uidh;	/* Unique Identification Register High */
+	u32 uidmh;	/* Unique Identification Register Mid-High */
+	u32 uidml;	/* Unique Identification Register Mid Low */
+	u32 uidl;	/* Unique Identification Register Low */
+	u32 clkdiv3;	/* System Clock Divider Register 3 */
+	u32 clkdiv4;	/* System Clock Divider Register 4 */
+	u32 mcr;	/* Misc Control Register */
+};
+#define KINETIS_SIM_PTR(base, reg) \
+	(&(((struct kinetis_sim_regs *)(base))->reg))
+
+/*
+ * System Clock Divider Register 1
+ */
+/* Clock 1 output divider value (for the core/system clock) */
+#define KINETIS_SIM_CLKDIV1_OUTDIV1_BITS	28
+#define KINETIS_SIM_CLKDIV1_OUTDIV1_MSK \
+	(((1 << 4) - 1) << KINETIS_SIM_CLKDIV1_OUTDIV1_BITS)
+/* Clock 2 output divider value (for the peripheral clock) */
+#define KINETIS_SIM_CLKDIV1_OUTDIV2_BITS	24
+#define KINETIS_SIM_CLKDIV1_OUTDIV2_MSK \
+	(((1 << 4) - 1) << KINETIS_SIM_CLKDIV1_OUTDIV2_BITS)
+
+/*
+ * System Clock Divider Register 2
+ */
+/* USB HS clock divider fraction */
+#define KINETIS_SIM_CLKDIV2_USBHSFRAC_BIT	8
+#define KINETIS_SIM_CLKDIV2_USBHSFRAC_MSK \
+	(1 << KINETIS_SIM_CLKDIV2_USBHSFRAC_BIT)
+/* USB HS clock divider divisor */
+#define KINETIS_SIM_CLKDIV2_USBHSDIV_BIT	9
+#define KINETIS_SIM_CLKDIV2_USBHSDIV_MSK \
+	(7 << KINETIS_SIM_CLKDIV2_USBHSDIV_BIT)
+
+/*
+ * MCG Control 5 Register
+ */
+/* PLL External Reference Divider */
+#define KINETIS_MCG_C5_PRDIV_BITS	0
+#define KINETIS_MCG_C5_PRDIV_MSK \
+	(((1 << 3) - 1) << KINETIS_MCG_C5_PRDIV_BITS)
+/* PLL Stop Enable */
+#define KINETIS_MCG_C5_PLLSTEN_MSK	(1 << 5)
+/* PLL Clock Enable */
+#define KINETIS_MCG_C5_PLLCLKEN_MSK	(1 << 6)
+/* PLL External Reference Select (for K70@120MHz) */
+#define KINETIS_MCG_C5_PLLREFSEL_BIT	7
+#define KINETIS_MCG_C5_PLLREFSEL_MSK	(1 << KINETIS_MCG_C5_PLLREFSEL_BIT)
+/*
+ * MCG Control 6 Register
+ */
+/* VCO Divider */
+#define KINETIS_MCG_C6_VDIV_BITS	0
+#define KINETIS_MCG_C6_VDIV_MSK \
+	(((1 << 5) - 1) << KINETIS_MCG_C6_VDIV_BITS)
+/* PLL Select */
+#define KINETIS_MCG_C6_PLLS_MSK		(1 << 6)
+/*
+ * MCG Control 11 Register
+ */
+/* PLL1 External Reference Divider */
+#define KINETIS_MCG_C11_PRDIV_BITS	0
+#define KINETIS_MCG_C11_PRDIV_MSK \
+	(((1 << 3) - 1) << KINETIS_MCG_C11_PRDIV_BITS)
+/* PLL Clock Select: PLL0 or PLL1 */
+#define KINETIS_MCG_C11_PLLCS_MSK	(1 << 4)
+/* PLL1 Stop Enable */
+#define KINETIS_MCG_C11_PLLSTEN1_MSK	(1 << 5)
+/* PLL1 Clock Enable */
+#define KINETIS_MCG_C11_PLLCLKEN1_MSK	(1 << 6)
+/* PLL1 External Reference Select (for K70@120MHz) */
+#define KINETIS_MCG_C11_PLLREFSEL1_BIT	7
+#define KINETIS_MCG_C11_PLLREFSEL1_MSK	(1 << KINETIS_MCG_C11_PLLREFSEL1_BIT)
+/*
+ * MCG Control 12 Register
+ */
+/* VCO1 Divider */
+#define KINETIS_MCG_C12_VDIV1_BITS	0
+#define KINETIS_MCG_C12_VDIV1_MSK \
+	(((1 << 5) - 1) << KINETIS_MCG_C12_VDIV1_BITS)
+
+/*
+ * Multipurpose Clock Generator (MCG) register map
+ *
+ * See Chapter 25 of the K70 Reference Manual
+ */
+struct kinetis_mcg_regs {
+	u8 c1;		/* MCG Control 1 Register */
+	u8 c2;		/* MCG Control 2 Register */
+	u8 c3;		/* MCG Control 3 Register */
+	u8 c4;		/* MCG Control 4 Register */
+	u8 c5;		/* MCG Control 5 Register */
+	u8 c6;		/* MCG Control 6 Register */
+	u8 status;	/* MCG Status Register */
+	u8 rsv0;
+	u8 atc;		/* MCG Auto Trim Control Register */
+	u8 rsv1;
+	u8 atcvh;	/* MCG Auto Trim Compare Value High Register */
+	u8 atcvl;	/* MCG Auto Trim Compare Value Low Register */
+	u8 c7;		/* MCG Control 7 Register */
+	u8 c8;		/* MCG Control 8 Register */
+	u8 rsv2;
+	u8 c10;		/* MCG Control 10 Register */
+	u8 c11;		/* MCG Control 11 Register */
+	u8 c12;		/* MCG Control 12 Register */
+	u8 status2;	/* MCG Status 2 Register */
+	u8 rsv3;
+};
+#define KINETIS_MCG_PTR(base, reg) \
+	(&(((struct kinetis_mcg_regs *)(base))->reg))
+
+struct kinetis_clk_gate {
+	const char *clk_gate_name;
+	struct clk *clk;
+	u32 reg;
+	u32 idx;
+	struct list_head clk_list;
+};
+
+struct kinetis_clk_gate_data {
+	void __iomem *sim;
+	struct list_head clk_gate_list;
+};
+
+struct kinetis_scgc {
+	unsigned long paddr;
+	spinlock_t lock;
+	struct list_head scgc_list;
+};
+
+static struct list_head kinetis_scgc_list = LIST_HEAD_INIT(kinetis_scgc_list);
+
+static const char *kinetis_of_clk_get_name(struct device_node *np)
+{
+	struct of_phandle_args clkspec;
+	int ret;
+	const char *retval;
+
+	ret = of_parse_phandle_with_args(np, "clocks", "#clock-cells", 0,
+					&clkspec);
+	if (ret)
+		return NULL;
+
+	retval = clkspec.np->full_name;
+
+	of_node_put(clkspec.np);
+
+	return retval;
+}
+
+static struct clk *kinetis_find_clk_gate(
+		struct kinetis_clk_gate_data *clk_gate_data, u32 reg, u32 idx)
+{
+	struct kinetis_clk_gate *gate;
+
+	list_for_each_entry(gate, &clk_gate_data->clk_gate_list, clk_list)
+		if ((gate->reg == reg) && (gate->idx == idx))
+			return gate->clk;
+
+	return NULL;
+}
+
+static struct kinetis_scgc *kinetis_find_scgc(unsigned long paddr)
+{
+	struct kinetis_scgc *scgc;
+
+	list_for_each_entry(scgc, &kinetis_scgc_list, scgc_list)
+		if (scgc->paddr == paddr)
+			return scgc;
+
+	return NULL;
+}
+
+static struct clk *kinetis_clk_gate_get(struct of_phandle_args *clkspec,
+					void *data)
+{
+	const char *clk_name;
+	struct kinetis_clk_gate_data *clk_gate_data = data;
+	struct kinetis_clk_gate *gate;
+	u32 reg = clkspec->args[0];
+	u32 idx = clkspec->args[1];
+	struct clk *clk;
+	struct kinetis_scgc *scgc;
+	void __iomem *addr = KINETIS_SIM_PTR(clk_gate_data->sim, scgc[reg]);
+	unsigned long paddr = virt_to_phys(addr);
+
+	clk = kinetis_find_clk_gate(clk_gate_data, reg, idx);
+	if (clk)
+		return clk;
+
+	clk_name = kinetis_of_clk_get_name(clkspec->np);
+	BUG_ON(!clk_name);
+
+	scgc = kinetis_find_scgc(paddr);
+	if (!scgc) {
+		scgc = kzalloc(sizeof(struct kinetis_scgc), GFP_KERNEL);
+		if (!scgc)
+			return ERR_PTR(-ENOMEM);
+		scgc->paddr = paddr;
+		spin_lock_init(&scgc->lock);
+
+		list_add(&scgc->scgc_list, &kinetis_scgc_list);
+	}
+
+	gate = kzalloc(sizeof(struct kinetis_clk_gate), GFP_KERNEL);
+	if (!gate)
+		return ERR_PTR(-ENOMEM);
+
+	gate->clk_gate_name = kasprintf(GFP_KERNEL, "%s:%u:%u",
+				clkspec->np->full_name, reg, idx);
+	if (!gate->clk_gate_name) {
+		kfree(gate);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	clk = clk_register_gate(NULL, gate->clk_gate_name, clk_name, 0, addr,
+							idx, 0, &scgc->lock);
+	if (IS_ERR(clk)) {
+		pr_err("Cannot register gate to clock %s\n", clk_name);
+		kfree_const(gate->clk_gate_name);
+		kfree(gate);
+		return clk;
+	}
+
+	gate->clk = clk;
+	gate->reg = reg;
+	gate->idx = idx;
+
+	list_add(&gate->clk_list, &clk_gate_data->clk_gate_list);
+
+	return clk;
+}
+
+static void kinetis_of_register_clk_gate(struct device_node *np)
+{
+	const char *clk_parent_name = kinetis_of_clk_get_name(np);
+	struct kinetis_clk_gate_data *clk_gate;
+	void __iomem *sim;
+	int ret;
+
+	if (!clk_parent_name) {
+		pr_err("no clock specified for gate %s\n", np->full_name);
+		return;
+	}
+
+	ret = of_property_match_string(np->parent, "reg-names", "sim");
+	if (ret < 0) {
+		pr_err("failed to get SIM address range for %s\n",
+							np->parent->full_name);
+		return;
+	}
+	sim = of_iomap(np->parent, ret);
+	if (!sim) {
+		pr_err("failed to map SIM address range for %s\n",
+							np->parent->full_name);
+		return;
+	}
+
+	clk_gate = kzalloc(sizeof(struct kinetis_clk_gate_data), GFP_KERNEL);
+	if (!clk_gate) {
+		iounmap(sim);
+		return;
+	}
+
+	clk_gate->sim = sim;
+	INIT_LIST_HEAD(&clk_gate->clk_gate_list);
+
+	ret = of_clk_add_provider(np, kinetis_clk_gate_get, clk_gate);
+	if (ret < 0) {
+		pr_err("Could not add clock provider %s\n", np->full_name);
+		kfree(clk_gate);
+		iounmap(sim);
+		return;
+	}
+}
+
+static void kinetis_of_register_fixed_rate(struct device_node *np, u32 clk_val)
+{
+	const char *clk_parent_name = kinetis_of_clk_get_name(np);
+	struct clk *clk;
+	int ret;
+
+	clk = clk_register_fixed_rate(NULL, np->full_name,
+					clk_parent_name,
+					clk_parent_name ? 0 : CLK_IS_ROOT,
+					clk_val);
+	if (IS_ERR(clk)) {
+		pr_err("Could not register clock %s\n", np->full_name);
+		return;
+	}
+
+	ret = of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	if (ret < 0) {
+		pr_err("Could not add clock provider %s\n", np->full_name);
+		clk_unregister(clk);
+		return;
+	}
+}
+
+static void __init kinetis_mcg_init(struct device_node *np)
+{
+	const int vco_div = 2;
+	const int vdiv_min = 16;
+	u32 clock_val[] = { [0 ... KINETIS_CLK_NUM] = 0 };
+	void __iomem *base;
+	void __iomem *sim;
+	int pll_sel;
+	int osc_sel;
+	int ret;
+	unsigned long mcgout;
+	enum kinetis_clk_ids clk_id;
+	u32 clock_cells;
+	struct device_node *child;
+
+	ret = of_property_match_string(np, "reg-names", "mcg");
+	if (ret < 0) {
+		pr_err("failed to get MCG address range for %s\n",
+								np->full_name);
+		return;
+	}
+	base = of_iomap(np, ret);
+	if (!base) {
+		pr_err("failed to map MCG address range for %s\n",
+								np->full_name);
+		return;
+	}
+
+	ret = of_property_match_string(np, "reg-names", "sim");
+	if (ret < 0) {
+		pr_err("failed to get SIM address range for %s\n",
+								np->full_name);
+		iounmap(base);
+		return;
+	}
+	sim = of_iomap(np, ret);
+	if (!sim) {
+		pr_err("failed to map SIM address range for %s\n",
+								np->full_name);
+		iounmap(base);
+		return;
+	}
+
+	/*
+	 * Check whether PLL0 or PLL1 is used for MCGOUTCLK
+	 */
+	pll_sel = !!(ioread8(KINETIS_MCG_PTR(base, c11)) &
+				KINETIS_MCG_C11_PLLCS_MSK);
+
+	/*
+	 * Check whether OSC0 or OSC1 is used to source the main PLL
+	 */
+	if (pll_sel)
+		osc_sel = !!(ioread8(KINETIS_MCG_PTR(base, c11)) &
+				KINETIS_MCG_C11_PLLREFSEL1_MSK);
+	else
+		osc_sel = !!(ioread8(KINETIS_MCG_PTR(base, c5)) &
+				KINETIS_MCG_C5_PLLREFSEL_MSK);
+
+	/*
+	 * Start with the MCG input clock
+	 */
+	mcgout = osc_sel ? KINETIS_OSC1_RATE : KINETIS_OSC0_RATE;
+
+	/*
+	 * Apply dividers and multipliers of the selected PLL
+	 */
+	if (pll_sel) {
+		/*
+		 * PLL1 internal divider (PRDIV)
+		 */
+		mcgout /= ((ioread8(KINETIS_MCG_PTR(base, c11)) &
+		  KINETIS_MCG_C11_PRDIV_MSK) >> KINETIS_MCG_C11_PRDIV_BITS) + 1;
+
+		/*
+		 * PLL1 multiplication factor (VDIV)
+		 */
+		mcgout *= ((ioread8(KINETIS_MCG_PTR(base, c12)) &
+		  KINETIS_MCG_C12_VDIV1_MSK) >> KINETIS_MCG_C12_VDIV1_BITS) +
+								    vdiv_min;
+	} else {
+		/*
+		 * PLL0 internal divider (PRDIV)
+		 */
+		mcgout /= ((ioread8(KINETIS_MCG_PTR(base, c5)) &
+			KINETIS_MCG_C5_PRDIV_MSK) >>
+			KINETIS_MCG_C5_PRDIV_BITS) + 1;
+
+		/*
+		 * PLL0 multiplication factor (VDIV)
+		 */
+		mcgout *= ((ioread8(KINETIS_MCG_PTR(base, c6)) &
+			KINETIS_MCG_C6_VDIV_MSK) >>
+			KINETIS_MCG_C6_VDIV_BITS) + vdiv_min;
+	}
+
+	/*
+	 * Apply the PLL output divider
+	 */
+	mcgout /= vco_div;
+
+	clock_val[KINETIS_CLK_MCGOUT] = mcgout;
+	clock_val[KINETIS_CLK_OSC0ER] = KINETIS_OSC0_RATE;
+
+	clock_val[KINETIS_CLK_CCLK] = mcgout /
+		(((ioread32(KINETIS_SIM_PTR(sim, clkdiv1)) &
+			KINETIS_SIM_CLKDIV1_OUTDIV1_MSK) >>
+				KINETIS_SIM_CLKDIV1_OUTDIV1_BITS) + 1);
+
+	/*
+	 * Peripheral (bus) clock
+	 */
+	clock_val[KINETIS_CLK_PCLK] = mcgout /
+		(((ioread32(KINETIS_SIM_PTR(sim, clkdiv1)) &
+			KINETIS_SIM_CLKDIV1_OUTDIV2_MSK) >>
+				KINETIS_SIM_CLKDIV1_OUTDIV2_BITS) + 1);
+
+	iounmap(sim);
+	iounmap(base);
+
+	for_each_child_of_node(np, child) {
+		if (!of_device_is_available(child))
+			continue;
+
+		clock_cells = 0;
+		if (of_property_read_u32_index(child, "#clock-cells", 0,
+							&clock_cells)) {
+			pr_err("no #clock-cells set for %s\n",
+							child->full_name);
+			continue;
+		}
+
+		/*
+		 * Handle clock gates (recognized by two clock cells)
+		 */
+		if (2 == clock_cells) {
+			kinetis_of_register_clk_gate(child);
+			continue;
+		}
+
+		for (clk_id = 0; clk_id < KINETIS_CLK_NUM; clk_id++)
+			if (!of_node_cmp(child->name,
+					    kinetis_clks[clk_id].name))
+				break;
+
+		if (KINETIS_CLK_NUM == clk_id) {
+			pr_err("unknown clock %s specified\n", child->name);
+			continue;
+		}
+
+		if (!clock_val[clk_id]) {
+			pr_err("no clock rate for %s\n", child->name);
+			continue;
+		}
+
+		kinetis_of_register_fixed_rate(child, clock_val[clk_id]);
+	}
+}
+
+CLK_OF_DECLARE(kinetis_mcg, "fsl,kinetis-cmu", kinetis_mcg_init);
-- 
2.3.6