ARM: Kirkwood: Replace clock gating

Add a varient of the basic clk-gate code. This variant calls a
function before gating the clock off. This function is used to disable
the SATA or PCIe PHY.

Now that all the drivers prepare and enable there clk as needed, there
is no need for the common code to keep track of which clocks need
gating on. Let the common clock framework turn off clocks which are
not used.

Buy using the added clk varient, when the clk framework turns off SATA
or PCIe clocks, we also disabled SATA and PCIe PHYs which were not
needed.

The function kirkwood_pcie_id() can now be called outside of __init
code, so remove this property for it, and functions it calls.

Signed-off-by: Andrew Lunn <andrew@lunn.ch>
Tested-by: Jamie Lentin <jm@lentin.co.uk>
Signed-off-by: Mike Turquette <mturquette@linaro.org>

2 files changed, 127 insertions, 90 deletions

diff --git a/arch/arm/mach-kirkwood/common.c b/arch/arm/mach-kirkwood/common.c
index 850ba64109cb..25fb3fd418ef 100644
--- a/arch/arm/mach-kirkwood/common.c
+++ b/arch/arm/mach-kirkwood/common.c
@@ -63,27 +63,136 @@ 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", CLK_IGNORE_UNUSED,
+        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;
@@ -95,15 +204,19 @@ void __init kirkwood_clk_init(void)
         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("sata0",  CGC_BIT_SATA0);
-        sata1 = kirkwood_register_gate("sata1",  CGC_BIT_SATA1);
+        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("pex0",   CGC_BIT_PEX0);
-        pex1 = kirkwood_register_gate("pex1",   CGC_BIT_PEX1);
+        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);
@@ -132,7 +245,6 @@ void __init kirkwood_clk_init(void)
  ****************************************************************************/
 void __init kirkwood_ehci_init(void)
 {
-        kirkwood_clk_ctrl |= CGC_USB0;
         orion_ehci_init(USB_PHYS_BASE, IRQ_KIRKWOOD_USB, EHCI_PHY_NA);
 }
 
@@ -142,8 +254,6 @@ 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);
@@ -155,9 +265,6 @@ 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);
@@ -202,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;
@@ -212,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;
@@ -233,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);
 }
 
@@ -279,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);
 }
@@ -290,7 +390,6 @@ 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);
 }
 
@@ -329,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);
 }
@@ -340,7 +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);
 }
@@ -351,7 +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);
 }
@@ -438,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);
 }
@@ -537,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)
 {
         /*
diff --git a/arch/arm/mach-kirkwood/pcie.c b/arch/arm/mach-kirkwood/pcie.c
index 881933a0b5eb..f26d6cff8bab 100644
--- a/arch/arm/mach-kirkwood/pcie.c
+++ b/arch/arm/mach-kirkwood/pcie.c
@@ -44,7 +44,7 @@ void kirkwood_enable_pcie(void)
                 writel(curr | CGC_PEX0, CLOCK_GATING_CTRL);
 }
 
-void __init kirkwood_pcie_id(u32 *dev, u32 *rev)
+void kirkwood_pcie_id(u32 *dev, u32 *rev)
 {
         kirkwood_enable_pcie();
         *dev = orion_pcie_dev_id((void __iomem *)PCIE_VIRT_BASE);
@@ -181,7 +181,6 @@ static void __init pcie1_ioresources_init(struct pcie_port *pp)
 
 static int __init kirkwood_pcie_setup(int nr, struct pci_sys_data *sys)
 {
-        extern unsigned int kirkwood_clk_ctrl;
         struct pcie_port *pp;
         int index;
 
@@ -200,12 +199,10 @@ static int __init kirkwood_pcie_setup(int nr, struct pci_sys_data *sys)
 
         switch (index) {
         case 0:
-                kirkwood_clk_ctrl |= CGC_PEX0;
                 kirkwood_enable_pcie_clk("0");
                 pcie0_ioresources_init(pp);
                 break;
         case 1:
-                kirkwood_clk_ctrl |= CGC_PEX1;
                 kirkwood_enable_pcie_clk("1");
                 pcie1_ioresources_init(pp);
                 break;