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;
diff --git a/arch/arm/plat-orion/pcie.c b/arch/arm/plat-orion/pcie.c
index 86dbb5bdb172..f20a321088a2 100644
--- a/arch/arm/plat-orion/pcie.c
+++ b/arch/arm/plat-orion/pcie.c
@@ -52,12 +52,12 @@
#define PCIE_DEBUG_SOFT_RESET (1<<20)
-u32 __init orion_pcie_dev_id(void __iomem *base)
+u32 orion_pcie_dev_id(void __iomem *base)
{
return readl(base + PCIE_DEV_ID_OFF) >> 16;
}
-u32 __init orion_pcie_rev(void __iomem *base)
+u32 orion_pcie_rev(void __iomem *base)
{
return readl(base + PCIE_DEV_REV_OFF) & 0xff;
}