1 files changed, 187 insertions, 87 deletions

diff --git a/arch/arm/mach-kirkwood/common.c b/arch/arm/mach-kirkwood/common.c
index a02cae881f2f..25fb3fd418ef 100644
--- a/arch/arm/mach-kirkwood/common.c
+++ b/arch/arm/mach-kirkwood/common.c
@@ -15,6 +15,8 @@
 #include <linux/ata_platform.h>
 #include <linux/mtd/nand.h>
 #include <linux/dma-mapping.h>
+#include <linux/clk-provider.h>
+#include <linux/spinlock.h>
 #include <net/dsa.h>
 #include <asm/page.h>
 #include <asm/timex.h>
@@ -31,6 +33,7 @@
 #include <plat/common.h>
 #include <plat/time.h>
 #include <plat/addr-map.h>
+#include <plat/mv_xor.h>
 #include "common.h"
 
 /*****************************************************************************
@@ -60,20 +63,188 @@ void __init kirkwood_map_io(void)
         iotable_init(kirkwood_io_desc, ARRAY_SIZE(kirkwood_io_desc));
 }
 
-/*
- * Default clock control bits.  Any bit _not_ set in this variable
- * will be cleared from the hardware after platform devices have been
- * registered.  Some reserved bits must be set to 1.
- */
-unsigned int kirkwood_clk_ctrl = CGC_DUNIT | CGC_RESERVED;
+/*****************************************************************************
+ * CLK tree
+ ****************************************************************************/
+
+static void disable_sata0(void)
+{
+        /* Disable PLL and IVREF */
+        writel(readl(SATA0_PHY_MODE_2) & ~0xf, SATA0_PHY_MODE_2);
+        /* Disable PHY */
+        writel(readl(SATA0_IF_CTRL) | 0x200, SATA0_IF_CTRL);
+}
+
+static void disable_sata1(void)
+{
+        /* Disable PLL and IVREF */
+        writel(readl(SATA1_PHY_MODE_2) & ~0xf, SATA1_PHY_MODE_2);
+        /* Disable PHY */
+        writel(readl(SATA1_IF_CTRL) | 0x200, SATA1_IF_CTRL);
+}
+
+static void disable_pcie0(void)
+{
+        writel(readl(PCIE_LINK_CTRL) | 0x10, PCIE_LINK_CTRL);
+        while (1)
+                if (readl(PCIE_STATUS) & 0x1)
+                        break;
+        writel(readl(PCIE_LINK_CTRL) & ~0x10, PCIE_LINK_CTRL);
+}
+
+static void disable_pcie1(void)
+{
+        u32 dev, rev;
+
+        kirkwood_pcie_id(&dev, &rev);
+
+        if (dev == MV88F6282_DEV_ID) {
+                writel(readl(PCIE1_LINK_CTRL) | 0x10, PCIE1_LINK_CTRL);
+                while (1)
+                        if (readl(PCIE1_STATUS) & 0x1)
+                                break;
+                writel(readl(PCIE1_LINK_CTRL) & ~0x10, PCIE1_LINK_CTRL);
+        }
+}
+
+/* An extended version of the gated clk. This calls fn() before
+ * disabling the clock. We use this to turn off PHYs etc. */
+struct clk_gate_fn {
+        struct clk_gate gate;
+        void (*fn)(void);
+};
 
+#define to_clk_gate_fn(_gate) container_of(_gate, struct clk_gate_fn, gate)
+#define to_clk_gate(_hw) container_of(_hw, struct clk_gate, hw)
+
+static void clk_gate_fn_disable(struct clk_hw *hw)
+{
+        struct clk_gate *gate = to_clk_gate(hw);
+        struct clk_gate_fn *gate_fn = to_clk_gate_fn(gate);
+
+        if (gate_fn->fn)
+                gate_fn->fn();
+
+        clk_gate_ops.disable(hw);
+}
+
+static struct clk_ops clk_gate_fn_ops;
+
+static struct clk __init *clk_register_gate_fn(struct device *dev,
+                const char *name,
+                const char *parent_name, unsigned long flags,
+                void __iomem *reg, u8 bit_idx,
+                u8 clk_gate_flags, spinlock_t *lock,
+                void (*fn)(void))
+{
+        struct clk_gate_fn *gate_fn;
+        struct clk *clk;
+        struct clk_init_data init;
+
+        gate_fn = kzalloc(sizeof(struct clk_gate_fn), GFP_KERNEL);
+        if (!gate_fn) {
+                pr_err("%s: could not allocate gated clk\n", __func__);
+                return ERR_PTR(-ENOMEM);
+        }
+
+        init.name = name;
+        init.ops = &clk_gate_fn_ops;
+        init.flags = flags;
+        init.parent_names = (parent_name ? &parent_name : NULL);
+        init.num_parents = (parent_name ? 1 : 0);
+
+        /* struct clk_gate assignments */
+        gate_fn->gate.reg = reg;
+        gate_fn->gate.bit_idx = bit_idx;
+        gate_fn->gate.flags = clk_gate_flags;
+        gate_fn->gate.lock = lock;
+        gate_fn->gate.hw.init = &init;
+
+        /* ops is the gate ops, but with our disable function */
+        if (clk_gate_fn_ops.disable != clk_gate_fn_disable) {
+                clk_gate_fn_ops = clk_gate_ops;
+                clk_gate_fn_ops.disable = clk_gate_fn_disable;
+        }
+
+        clk = clk_register(dev, &gate_fn->gate.hw);
+
+        if (IS_ERR(clk))
+                kfree(gate_fn);
+
+        return clk;
+}
+
+static DEFINE_SPINLOCK(gating_lock);
+static struct clk *tclk;
+
+static struct clk __init *kirkwood_register_gate(const char *name, u8 bit_idx)
+{
+        return clk_register_gate(NULL, name, "tclk", 0,
+                                 (void __iomem *)CLOCK_GATING_CTRL,
+                                 bit_idx, 0, &gating_lock);
+}
+
+static struct clk __init *kirkwood_register_gate_fn(const char *name,
+                                                    u8 bit_idx,
+                                                    void (*fn)(void))
+{
+        return clk_register_gate_fn(NULL, name, "tclk", 0,
+                                    (void __iomem *)CLOCK_GATING_CTRL,
+                                    bit_idx, 0, &gating_lock, fn);
+}
+
+void __init kirkwood_clk_init(void)
+{
+        struct clk *runit, *ge0, *ge1, *sata0, *sata1, *usb0, *sdio;
+        struct clk *crypto, *xor0, *xor1, *pex0, *pex1, *audio;
+
+        tclk = clk_register_fixed_rate(NULL, "tclk", NULL,
+                                       CLK_IS_ROOT, kirkwood_tclk);
+
+        runit = kirkwood_register_gate("runit",  CGC_BIT_RUNIT);
+        ge0 = kirkwood_register_gate("ge0",    CGC_BIT_GE0);
+        ge1 = kirkwood_register_gate("ge1",    CGC_BIT_GE1);
+        sata0 = kirkwood_register_gate_fn("sata0",  CGC_BIT_SATA0,
+                                          disable_sata0);
+        sata1 = kirkwood_register_gate_fn("sata1",  CGC_BIT_SATA1,
+                                          disable_sata1);
+        usb0 = kirkwood_register_gate("usb0",   CGC_BIT_USB0);
+        sdio = kirkwood_register_gate("sdio",   CGC_BIT_SDIO);
+        crypto = kirkwood_register_gate("crypto", CGC_BIT_CRYPTO);
+        xor0 = kirkwood_register_gate("xor0",   CGC_BIT_XOR0);
+        xor1 = kirkwood_register_gate("xor1",   CGC_BIT_XOR1);
+        pex0 = kirkwood_register_gate_fn("pex0",   CGC_BIT_PEX0,
+                                         disable_pcie0);
+        pex1 = kirkwood_register_gate_fn("pex1",   CGC_BIT_PEX1,
+                                         disable_pcie1);
+        audio = kirkwood_register_gate("audio",  CGC_BIT_AUDIO);
+        kirkwood_register_gate("tdm",    CGC_BIT_TDM);
+        kirkwood_register_gate("tsu",    CGC_BIT_TSU);
+
+        /* clkdev entries, mapping clks to devices */
+        orion_clkdev_add(NULL, "orion_spi.0", runit);
+        orion_clkdev_add(NULL, "orion_spi.1", runit);
+        orion_clkdev_add(NULL, MV643XX_ETH_NAME ".0", ge0);
+        orion_clkdev_add(NULL, MV643XX_ETH_NAME ".1", ge1);
+        orion_clkdev_add(NULL, "orion_wdt", tclk);
+        orion_clkdev_add("0", "sata_mv.0", sata0);
+        orion_clkdev_add("1", "sata_mv.0", sata1);
+        orion_clkdev_add(NULL, "orion-ehci.0", usb0);
+        orion_clkdev_add(NULL, "orion_nand", runit);
+        orion_clkdev_add(NULL, "mvsdio", sdio);
+        orion_clkdev_add(NULL, "mv_crypto", crypto);
+        orion_clkdev_add(NULL, MV_XOR_SHARED_NAME ".0", xor0);
+        orion_clkdev_add(NULL, MV_XOR_SHARED_NAME ".1", xor1);
+        orion_clkdev_add("0", "pcie", pex0);
+        orion_clkdev_add("1", "pcie", pex1);
+        orion_clkdev_add(NULL, "kirkwood-i2s", audio);
+}
 
 /*****************************************************************************
  * EHCI0
  ****************************************************************************/
 void __init kirkwood_ehci_init(void)
 {
-        kirkwood_clk_ctrl |= CGC_USB0;
         orion_ehci_init(USB_PHYS_BASE, IRQ_KIRKWOOD_USB, EHCI_PHY_NA);
 }
 
@@ -83,11 +254,9 @@ void __init kirkwood_ehci_init(void)
  ****************************************************************************/
 void __init kirkwood_ge00_init(struct mv643xx_eth_platform_data *eth_data)
 {
-        kirkwood_clk_ctrl |= CGC_GE0;
-
         orion_ge00_init(eth_data,
                         GE00_PHYS_BASE, IRQ_KIRKWOOD_GE00_SUM,
-                        IRQ_KIRKWOOD_GE00_ERR, kirkwood_tclk);
+                        IRQ_KIRKWOOD_GE00_ERR);
 }
 
 
@@ -96,12 +265,9 @@ void __init kirkwood_ge00_init(struct mv643xx_eth_platform_data *eth_data)
  ****************************************************************************/
 void __init kirkwood_ge01_init(struct mv643xx_eth_platform_data *eth_data)
 {
-
-        kirkwood_clk_ctrl |= CGC_GE1;
-
         orion_ge01_init(eth_data,
                         GE01_PHYS_BASE, IRQ_KIRKWOOD_GE01_SUM,
-                        IRQ_KIRKWOOD_GE01_ERR, kirkwood_tclk);
+                        IRQ_KIRKWOOD_GE01_ERR);
 }
 
 
@@ -143,7 +309,6 @@ static struct platform_device kirkwood_nand_flash = {
 void __init kirkwood_nand_init(struct mtd_partition *parts, int nr_parts,
                                int chip_delay)
 {
-        kirkwood_clk_ctrl |= CGC_RUNIT;
         kirkwood_nand_data.parts = parts;
         kirkwood_nand_data.nr_parts = nr_parts;
         kirkwood_nand_data.chip_delay = chip_delay;
@@ -153,7 +318,6 @@ void __init kirkwood_nand_init(struct mtd_partition *parts, int nr_parts,
 void __init kirkwood_nand_init_rnb(struct mtd_partition *parts, int nr_parts,
                                    int (*dev_ready)(struct mtd_info *))
 {
-        kirkwood_clk_ctrl |= CGC_RUNIT;
         kirkwood_nand_data.parts = parts;
         kirkwood_nand_data.nr_parts = nr_parts;
         kirkwood_nand_data.dev_ready = dev_ready;
@@ -174,10 +338,6 @@ static void __init kirkwood_rtc_init(void)
  ****************************************************************************/
 void __init kirkwood_sata_init(struct mv_sata_platform_data *sata_data)
 {
-        kirkwood_clk_ctrl |= CGC_SATA0;
-        if (sata_data->n_ports > 1)
-                kirkwood_clk_ctrl |= CGC_SATA1;
-
         orion_sata_init(sata_data, SATA_PHYS_BASE, IRQ_KIRKWOOD_SATA);
 }
 
@@ -220,7 +380,6 @@ void __init kirkwood_sdio_init(struct mvsdio_platform_data *mvsdio_data)
                 mvsdio_data->clock = 100000000;
         else
                 mvsdio_data->clock = 200000000;
-        kirkwood_clk_ctrl |= CGC_SDIO;
         kirkwood_sdio.dev.platform_data = mvsdio_data;
         platform_device_register(&kirkwood_sdio);
 }
@@ -231,8 +390,7 @@ void __init kirkwood_sdio_init(struct mvsdio_platform_data *mvsdio_data)
  ****************************************************************************/
 void __init kirkwood_spi_init()
 {
-        kirkwood_clk_ctrl |= CGC_RUNIT;
-        orion_spi_init(SPI_PHYS_BASE, kirkwood_tclk);
+        orion_spi_init(SPI_PHYS_BASE);
 }
 
 
@@ -252,7 +410,7 @@ void __init kirkwood_i2c_init(void)
 void __init kirkwood_uart0_init(void)
 {
         orion_uart0_init(UART0_VIRT_BASE, UART0_PHYS_BASE,
-                         IRQ_KIRKWOOD_UART_0, kirkwood_tclk);
+                         IRQ_KIRKWOOD_UART_0, tclk);
 }
 
 
@@ -262,7 +420,7 @@ void __init kirkwood_uart0_init(void)
 void __init kirkwood_uart1_init(void)
 {
         orion_uart1_init(UART1_VIRT_BASE, UART1_PHYS_BASE,
-                         IRQ_KIRKWOOD_UART_1, kirkwood_tclk);
+                         IRQ_KIRKWOOD_UART_1, tclk);
 }
 
 /*****************************************************************************
@@ -270,7 +428,6 @@ void __init kirkwood_uart1_init(void)
  ****************************************************************************/
 void __init kirkwood_crypto_init(void)
 {
-        kirkwood_clk_ctrl |= CGC_CRYPTO;
         orion_crypto_init(CRYPTO_PHYS_BASE, KIRKWOOD_SRAM_PHYS_BASE,
                           KIRKWOOD_SRAM_SIZE, IRQ_KIRKWOOD_CRYPTO);
 }
@@ -281,8 +438,6 @@ void __init kirkwood_crypto_init(void)
  ****************************************************************************/
 void __init kirkwood_xor0_init(void)
 {
-        kirkwood_clk_ctrl |= CGC_XOR0;
-
         orion_xor0_init(XOR0_PHYS_BASE, XOR0_HIGH_PHYS_BASE,
                         IRQ_KIRKWOOD_XOR_00, IRQ_KIRKWOOD_XOR_01);
 }
@@ -293,8 +448,6 @@ void __init kirkwood_xor0_init(void)
  ****************************************************************************/
 void __init kirkwood_xor1_init(void)
 {
-        kirkwood_clk_ctrl |= CGC_XOR1;
-
         orion_xor1_init(XOR1_PHYS_BASE, XOR1_HIGH_PHYS_BASE,
                         IRQ_KIRKWOOD_XOR_10, IRQ_KIRKWOOD_XOR_11);
 }
@@ -305,7 +458,7 @@ void __init kirkwood_xor1_init(void)
  ****************************************************************************/
 void __init kirkwood_wdt_init(void)
 {
-        orion_wdt_init(kirkwood_tclk);
+        orion_wdt_init();
 }
 
 
@@ -381,7 +534,6 @@ static struct platform_device kirkwood_pcm_device = {
 
 void __init kirkwood_audio_init(void)
 {
-        kirkwood_clk_ctrl |= CGC_AUDIO;
         platform_device_register(&kirkwood_i2s_device);
         platform_device_register(&kirkwood_pcm_device);
 }
@@ -465,6 +617,9 @@ void __init kirkwood_init(void)
         kirkwood_l2_init();
 #endif
 
+        /* Setup root of clk tree */
+        kirkwood_clk_init();
+
         /* internal devices that every board has */
         kirkwood_rtc_init();
         kirkwood_wdt_init();
@@ -477,61 +632,6 @@ void __init kirkwood_init(void)
 #endif
 }
 
-static int __init kirkwood_clock_gate(void)
-{
-        unsigned int curr = readl(CLOCK_GATING_CTRL);
-        u32 dev, rev;
-
-        kirkwood_pcie_id(&dev, &rev);
-        printk(KERN_DEBUG "Gating clock of unused units\n");
-        printk(KERN_DEBUG "before: 0x%08x\n", curr);
-
-        /* Make sure those units are accessible */
-        writel(curr | CGC_SATA0 | CGC_SATA1 | CGC_PEX0 | CGC_PEX1, CLOCK_GATING_CTRL);
-
-        /* For SATA: first shutdown the phy */
-        if (!(kirkwood_clk_ctrl & CGC_SATA0)) {
-                /* Disable PLL and IVREF */
-                writel(readl(SATA0_PHY_MODE_2) & ~0xf, SATA0_PHY_MODE_2);
-                /* Disable PHY */
-                writel(readl(SATA0_IF_CTRL) | 0x200, SATA0_IF_CTRL);
-        }
-        if (!(kirkwood_clk_ctrl & CGC_SATA1)) {
-                /* Disable PLL and IVREF */
-                writel(readl(SATA1_PHY_MODE_2) & ~0xf, SATA1_PHY_MODE_2);
-                /* Disable PHY */
-                writel(readl(SATA1_IF_CTRL) | 0x200, SATA1_IF_CTRL);
-        }
-        
-        /* For PCIe: first shutdown the phy */
-        if (!(kirkwood_clk_ctrl & CGC_PEX0)) {
-                writel(readl(PCIE_LINK_CTRL) | 0x10, PCIE_LINK_CTRL);
-                while (1)
-                        if (readl(PCIE_STATUS) & 0x1)
-                                break;
-                writel(readl(PCIE_LINK_CTRL) & ~0x10, PCIE_LINK_CTRL);
-        }
-
-        /* For PCIe 1: first shutdown the phy */
-        if (dev == MV88F6282_DEV_ID) {
-                if (!(kirkwood_clk_ctrl & CGC_PEX1)) {
-                        writel(readl(PCIE1_LINK_CTRL) | 0x10, PCIE1_LINK_CTRL);
-                        while (1)
-                                if (readl(PCIE1_STATUS) & 0x1)
-                                        break;
-                        writel(readl(PCIE1_LINK_CTRL) & ~0x10, PCIE1_LINK_CTRL);
-                }
-        } else  /* keep this bit set for devices that don't have PCIe1 */
-                kirkwood_clk_ctrl |= CGC_PEX1;
-
-        /* Now gate clock the required units */
-        writel(kirkwood_clk_ctrl, CLOCK_GATING_CTRL);
-        printk(KERN_DEBUG " after: 0x%08x\n", readl(CLOCK_GATING_CTRL));
-
-        return 0;
-}
-late_initcall(kirkwood_clock_gate);
-
 void kirkwood_restart(char mode, const char *cmd)
 {
         /*