8 files changed, 750 insertions, 31 deletions

diff --git a/arch/arm/mach-davinci/Makefile b/arch/arm/mach-davinci/Makefile
index a8f88cd2990..99ac2e55774 100644
--- a/arch/arm/mach-davinci/Makefile
+++ b/arch/arm/mach-davinci/Makefile
@@ -4,7 +4,8 @@
 #
 
 # Common objects
-obj-y 			:= time.o irq.o serial.o io.o id.o psc.o
+obj-y 			:= time.o irq.o clock.o serial.o io.o id.o psc.o \
+			   gpio.o mux.o
 
 # Board specific
 obj-$(CONFIG_MACH_DAVINCI_EVM)  += board-evm.o
diff --git a/arch/arm/mach-davinci/board-evm.c b/arch/arm/mach-davinci/board-evm.c
index 633c12e4304..9e4024c4965 100644
--- a/arch/arm/mach-davinci/board-evm.c
+++ b/arch/arm/mach-davinci/board-evm.c
@@ -32,6 +32,7 @@
 void __init davinci_psc_init(void);
 void __init davinci_irq_init(void);
 void __init davinci_map_common_io(void);
+void __init davinci_init_common_hw(void);
 
 /* NOR Flash base address set to CS0 by default */
 #define NOR_FLASH_PHYS 0x02000000
@@ -116,6 +117,7 @@ static __init void davinci_evm_init(void)
 
 static __init void davinci_evm_irq_init(void)
 {
+	davinci_init_common_hw();
 	davinci_irq_init();
 }
 
diff --git a/arch/arm/mach-davinci/clock.c b/arch/arm/mach-davinci/clock.c
new file mode 100644
index 00000000000..139ceaa35e2
--- /dev/null
+++ b/arch/arm/mach-davinci/clock.c
@@ -0,0 +1,323 @@
+/*
+ * TI DaVinci clock config file
+ *
+ * Copyright (C) 2006 Texas Instruments.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+
+#include <asm/hardware.h>
+#include <asm/io.h>
+
+#include <asm/arch/psc.h>
+#include "clock.h"
+
+/* PLL/Reset register offsets */
+#define PLLM		0x110
+
+static LIST_HEAD(clocks);
+static DEFINE_MUTEX(clocks_mutex);
+static DEFINE_SPINLOCK(clockfw_lock);
+
+static unsigned int commonrate;
+static unsigned int armrate;
+static unsigned int fixedrate = 27000000;	/* 27 MHZ */
+
+extern void davinci_psc_config(unsigned int domain, unsigned int id, char enable);
+
+/*
+ * Returns a clock. Note that we first try to use device id on the bus
+ * and clock name. If this fails, we try to use clock name only.
+ */
+struct clk *clk_get(struct device *dev, const char *id)
+{
+	struct clk *p, *clk = ERR_PTR(-ENOENT);
+	int idno;
+
+	if (dev == NULL || dev->bus != &platform_bus_type)
+		idno = -1;
+	else
+		idno = to_platform_device(dev)->id;
+
+	mutex_lock(&clocks_mutex);
+
+	list_for_each_entry(p, &clocks, node) {
+		if (p->id == idno &&
+		    strcmp(id, p->name) == 0 && try_module_get(p->owner)) {
+			clk = p;
+			goto found;
+		}
+	}
+
+	list_for_each_entry(p, &clocks, node) {
+		if (strcmp(id, p->name) == 0 && try_module_get(p->owner)) {
+			clk = p;
+			break;
+		}
+	}
+
+found:
+	mutex_unlock(&clocks_mutex);
+
+	return clk;
+}
+EXPORT_SYMBOL(clk_get);
+
+void clk_put(struct clk *clk)
+{
+	if (clk && !IS_ERR(clk))
+		module_put(clk->owner);
+}
+EXPORT_SYMBOL(clk_put);
+
+static int __clk_enable(struct clk *clk)
+{
+	if (clk->flags & ALWAYS_ENABLED)
+		return 0;
+
+	davinci_psc_config(DAVINCI_GPSC_ARMDOMAIN, clk->lpsc, 1);
+	return 0;
+}
+
+static void __clk_disable(struct clk *clk)
+{
+	if (clk->usecount)
+		return;
+
+	davinci_psc_config(DAVINCI_GPSC_ARMDOMAIN, clk->lpsc, 0);
+}
+
+int clk_enable(struct clk *clk)
+{
+	unsigned long flags;
+	int ret = 0;
+
+	if (clk == NULL || IS_ERR(clk))
+		return -EINVAL;
+
+	if (clk->usecount++ == 0) {
+		spin_lock_irqsave(&clockfw_lock, flags);
+		ret = __clk_enable(clk);
+		spin_unlock_irqrestore(&clockfw_lock, flags);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(clk_enable);
+
+void clk_disable(struct clk *clk)
+{
+	unsigned long flags;
+
+	if (clk == NULL || IS_ERR(clk))
+		return;
+
+	if (clk->usecount > 0 && !(--clk->usecount)) {
+		spin_lock_irqsave(&clockfw_lock, flags);
+		__clk_disable(clk);
+		spin_unlock_irqrestore(&clockfw_lock, flags);
+	}
+}
+EXPORT_SYMBOL(clk_disable);
+
+unsigned long clk_get_rate(struct clk *clk)
+{
+	if (clk == NULL || IS_ERR(clk))
+		return -EINVAL;
+
+	return *(clk->rate);
+}
+EXPORT_SYMBOL(clk_get_rate);
+
+long clk_round_rate(struct clk *clk, unsigned long rate)
+{
+	if (clk == NULL || IS_ERR(clk))
+		return -EINVAL;
+
+	return *(clk->rate);
+}
+EXPORT_SYMBOL(clk_round_rate);
+
+int clk_set_rate(struct clk *clk, unsigned long rate)
+{
+	if (clk == NULL || IS_ERR(clk))
+		return -EINVAL;
+
+	/* changing the clk rate is not supported */
+	return -EINVAL;
+}
+EXPORT_SYMBOL(clk_set_rate);
+
+int clk_register(struct clk *clk)
+{
+	if (clk == NULL || IS_ERR(clk))
+		return -EINVAL;
+
+	mutex_lock(&clocks_mutex);
+	list_add(&clk->node, &clocks);
+	mutex_unlock(&clocks_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL(clk_register);
+
+void clk_unregister(struct clk *clk)
+{
+	if (clk == NULL || IS_ERR(clk))
+		return;
+
+	mutex_lock(&clocks_mutex);
+	list_del(&clk->node);
+	mutex_unlock(&clocks_mutex);
+}
+EXPORT_SYMBOL(clk_unregister);
+
+static struct clk davinci_clks[] = {
+	{
+		.name = "ARMCLK",
+		.rate = &armrate,
+		.lpsc = -1,
+		.flags = ALWAYS_ENABLED,
+	},
+	{
+		.name = "UART",
+		.rate = &fixedrate,
+		.lpsc = DAVINCI_LPSC_UART0,
+	},
+	{
+		.name = "EMACCLK",
+		.rate = &commonrate,
+		.lpsc = DAVINCI_LPSC_EMAC_WRAPPER,
+	},
+	{
+		.name = "I2CCLK",
+		.rate = &fixedrate,
+		.lpsc = DAVINCI_LPSC_I2C,
+	},
+	{
+		.name = "IDECLK",
+		.rate = &commonrate,
+		.lpsc = DAVINCI_LPSC_ATA,
+	},
+	{
+		.name = "McBSPCLK",
+		.rate = &commonrate,
+		.lpsc = DAVINCI_LPSC_McBSP,
+	},
+	{
+		.name = "MMCSDCLK",
+		.rate = &commonrate,
+		.lpsc = DAVINCI_LPSC_MMC_SD,
+	},
+	{
+		.name = "SPICLK",
+		.rate = &commonrate,
+		.lpsc = DAVINCI_LPSC_SPI,
+	},
+	{
+		.name = "gpio",
+		.rate = &commonrate,
+		.lpsc = DAVINCI_LPSC_GPIO,
+	},
+	{
+		.name = "AEMIFCLK",
+		.rate = &commonrate,
+		.lpsc = DAVINCI_LPSC_AEMIF,
+		.usecount = 1,
+	}
+};
+
+int __init davinci_clk_init(void)
+{
+	struct clk *clkp;
+	int count = 0;
+	u32 pll_mult;
+
+	pll_mult = davinci_readl(DAVINCI_PLL_CNTRL0_BASE + PLLM);
+	commonrate = ((pll_mult + 1) * 27000000) / 6;
+	armrate = ((pll_mult + 1) * 27000000) / 2;
+
+	for (clkp = davinci_clks; count < ARRAY_SIZE(davinci_clks);
+	     count++, clkp++) {
+		clk_register(clkp);
+
+		/* Turn on clocks that have been enabled in the
+		 * table above */
+		if (clkp->usecount)
+			clk_enable(clkp);
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PROC_FS
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+
+static void *davinci_ck_start(struct seq_file *m, loff_t *pos)
+{
+	return *pos < 1 ? (void *)1 : NULL;
+}
+
+static void *davinci_ck_next(struct seq_file *m, void *v, loff_t *pos)
+{
+	++*pos;
+	return NULL;
+}
+
+static void davinci_ck_stop(struct seq_file *m, void *v)
+{
+}
+
+static int davinci_ck_show(struct seq_file *m, void *v)
+{
+	struct clk *cp;
+
+	list_for_each_entry(cp, &clocks, node)
+		seq_printf(m,"%s %d %d\n", cp->name, *(cp->rate), cp->usecount);
+
+	return 0;
+}
+
+static struct seq_operations davinci_ck_op = {
+	.start	= davinci_ck_start,
+	.next	= davinci_ck_next,
+	.stop	= davinci_ck_stop,
+	.show	= davinci_ck_show
+};
+
+static int davinci_ck_open(struct inode *inode, struct file *file)
+{
+	return seq_open(file, &davinci_ck_op);
+}
+
+static struct file_operations proc_davinci_ck_operations = {
+	.open		= davinci_ck_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= seq_release,
+};
+
+static int __init davinci_ck_proc_init(void)
+{
+	struct proc_dir_entry *entry;
+
+	entry = create_proc_entry("davinci_clocks", 0, NULL);
+	if (entry)
+		entry->proc_fops = &proc_davinci_ck_operations;
+	return 0;
+
+}
+__initcall(davinci_ck_proc_init);
+#endif	/* CONFIG_DEBUG_PROC_FS */
diff --git a/arch/arm/mach-davinci/clock.h b/arch/arm/mach-davinci/clock.h
new file mode 100644
index 00000000000..ed47079a52e
--- /dev/null
+++ b/arch/arm/mach-davinci/clock.h
@@ -0,0 +1,33 @@
+/*
+ * TI DaVinci clock definitions
+ *
+ * Copyright (C) 2006 Texas Instruments.
+ *
+ * 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.
+ */
+
+#ifndef __ARCH_ARM_DAVINCI_CLOCK_H
+#define __ARCH_ARM_DAVINCI_CLOCK_H
+
+struct clk {
+	struct list_head	node;
+	struct module		*owner;
+	const char		*name;
+	unsigned int		*rate;
+	int			id;
+	__s8			usecount;
+	__u8			flags;
+	__u8			lpsc;
+};
+
+/* Clock flags */
+#define RATE_CKCTL		1
+#define RATE_FIXED		2
+#define RATE_PROPAGATES		4
+#define VIRTUAL_CLOCK		8
+#define ALWAYS_ENABLED		16
+#define ENABLE_REG_32BIT	32
+
+#endif
diff --git a/arch/arm/mach-davinci/gpio.c b/arch/arm/mach-davinci/gpio.c
new file mode 100644
index 00000000000..9c67886e718
--- /dev/null
+++ b/arch/arm/mach-davinci/gpio.c
@@ -0,0 +1,286 @@
+/*
+ * TI DaVinci GPIO Support
+ *
+ * Copyright (c) 2006 David Brownell
+ * Copyright (c) 2007, MontaVista Software, Inc. <source@mvista.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/bitops.h>
+
+#include <asm/arch/irqs.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/gpio.h>
+
+#include <asm/mach/irq.h>
+
+static DEFINE_SPINLOCK(gpio_lock);
+static DECLARE_BITMAP(gpio_in_use, DAVINCI_N_GPIO);
+
+int gpio_request(unsigned gpio, const char *tag)
+{
+	if (gpio >= DAVINCI_N_GPIO)
+		return -EINVAL;
+
+	if (test_and_set_bit(gpio, gpio_in_use))
+		return -EBUSY;
+
+	return 0;
+}
+EXPORT_SYMBOL(gpio_request);
+
+void gpio_free(unsigned gpio)
+{
+	if (gpio >= DAVINCI_N_GPIO)
+		return;
+
+	clear_bit(gpio, gpio_in_use);
+}
+EXPORT_SYMBOL(gpio_free);
+
+/* create a non-inlined version */
+static struct gpio_controller *__iomem gpio2controller(unsigned gpio)
+{
+	return __gpio_to_controller(gpio);
+}
+
+/*
+ * Assuming the pin is muxed as a gpio output, set its output value.
+ */
+void __gpio_set(unsigned gpio, int value)
+{
+	struct gpio_controller *__iomem g = gpio2controller(gpio);
+
+	__raw_writel(__gpio_mask(gpio), value ? &g->set_data : &g->clr_data);
+}
+EXPORT_SYMBOL(__gpio_set);
+
+
+/*
+ * Read the pin's value (works even if it's set up as output);
+ * returns zero/nonzero.
+ *
+ * Note that changes are synched to the GPIO clock, so reading values back
+ * right after you've set them may give old values.
+ */
+int __gpio_get(unsigned gpio)
+{
+	struct gpio_controller *__iomem g = gpio2controller(gpio);
+
+	return !!(__gpio_mask(gpio) & __raw_readl(&g->in_data));
+}
+EXPORT_SYMBOL(__gpio_get);
+
+
+/*--------------------------------------------------------------------------*/
+
+/*
+ * board setup code *MUST* set PINMUX0 and PINMUX1 as
+ * needed, and enable the GPIO clock.
+ */
+
+int gpio_direction_input(unsigned gpio)
+{
+	struct gpio_controller *__iomem g = gpio2controller(gpio);
+	u32 temp;
+	u32 mask;
+
+	if (!g)
+		return -EINVAL;
+
+	spin_lock(&gpio_lock);
+	mask = __gpio_mask(gpio);
+	temp = __raw_readl(&g->dir);
+	temp |= mask;
+	__raw_writel(temp, &g->dir);
+	spin_unlock(&gpio_lock);
+	return 0;
+}
+EXPORT_SYMBOL(gpio_direction_input);
+
+int gpio_direction_output(unsigned gpio, int value)
+{
+	struct gpio_controller *__iomem g = gpio2controller(gpio);
+	u32 temp;
+	u32 mask;
+
+	if (!g)
+		return -EINVAL;
+
+	spin_lock(&gpio_lock);
+	mask = __gpio_mask(gpio);
+	temp = __raw_readl(&g->dir);
+	temp &= ~mask;
+	__raw_writel(mask, value ? &g->set_data : &g->clr_data);
+	__raw_writel(temp, &g->dir);
+	spin_unlock(&gpio_lock);
+	return 0;
+}
+EXPORT_SYMBOL(gpio_direction_output);
+
+/*
+ * We expect irqs will normally be set up as input pins, but they can also be
+ * used as output pins ... which is convenient for testing.
+ *
+ * NOTE:  GPIO0..GPIO7 also have direct INTC hookups, which work in addition
+ * to their GPIOBNK0 irq (but with a bit less overhead).  But we don't have
+ * a good way to hook those up ...
+ *
+ * All those INTC hookups (GPIO0..GPIO7 plus five IRQ banks) can also
+ * serve as EDMA event triggers.
+ */
+
+static void gpio_irq_disable(unsigned irq)
+{
+	struct gpio_controller *__iomem g = get_irq_chip_data(irq);
+	u32 mask = __gpio_mask(irq_to_gpio(irq));
+
+	__raw_writel(mask, &g->clr_falling);
+	__raw_writel(mask, &g->clr_rising);
+}
+
+static void gpio_irq_enable(unsigned irq)
+{
+	struct gpio_controller *__iomem g = get_irq_chip_data(irq);
+	u32 mask = __gpio_mask(irq_to_gpio(irq));
+
+	if (irq_desc[irq].status & IRQ_TYPE_EDGE_FALLING)
+		__raw_writel(mask, &g->set_falling);
+	if (irq_desc[irq].status & IRQ_TYPE_EDGE_RISING)
+		__raw_writel(mask, &g->set_rising);
+}
+
+static int gpio_irq_type(unsigned irq, unsigned trigger)
+{
+	struct gpio_controller *__iomem g = get_irq_chip_data(irq);
+	u32 mask = __gpio_mask(irq_to_gpio(irq));
+
+	if (trigger & ~(IRQ_TYPE_EDGE_FALLING | IRQ_TYPE_EDGE_RISING))
+		return -EINVAL;
+
+	irq_desc[irq].status &= ~IRQ_TYPE_SENSE_MASK;
+	irq_desc[irq].status |= trigger;
+
+	__raw_writel(mask, (trigger & IRQ_TYPE_EDGE_FALLING)
+		     ? &g->set_falling : &g->clr_falling);
+	__raw_writel(mask, (trigger & IRQ_TYPE_EDGE_RISING)
+		     ? &g->set_rising : &g->clr_rising);
+	return 0;
+}
+
+static struct irq_chip gpio_irqchip = {
+	.name		= "GPIO",
+	.enable		= gpio_irq_enable,
+	.disable	= gpio_irq_disable,
+	.set_type	= gpio_irq_type,
+};
+
+static void
+gpio_irq_handler(unsigned irq, struct irq_desc *desc)
+{
+	struct gpio_controller *__iomem g = get_irq_chip_data(irq);
+	u32 mask = 0xffff;
+
+	/* we only care about one bank */
+	if (irq & 1)
+		mask <<= 16;
+
+	/* temporarily mask (level sensitive) parent IRQ */
+	desc->chip->ack(irq);
+	while (1) {
+		u32		status;
+		struct irq_desc	*gpio;
+		int		n;
+		int		res;
+
+		/* ack any irqs */
+		status = __raw_readl(&g->intstat) & mask;
+		if (!status)
+			break;
+		__raw_writel(status, &g->intstat);
+		if (irq & 1)
+			status >>= 16;
+
+		/* now demux them to the right lowlevel handler */
+		n = (int)get_irq_data(irq);
+		gpio = &irq_desc[n];
+		while (status) {
+			res = ffs(status);
+			n += res;
+			gpio += res;
+			desc_handle_irq(n - 1, gpio - 1);
+			status >>= res;
+		}
+	}
+	desc->chip->unmask(irq);
+	/* now it may re-trigger */
+}
+
+/*
+ * NOTE:  for suspend/resume, probably best to make a sysdev (and class)
+ * with its suspend/resume calls hooking into the results of the set_wake()
+ * calls ... so if no gpios are wakeup events the clock can be disabled,
+ * with outputs left at previously set levels, and so that VDD3P3V.IOPWDN0
+ * can be set appropriately for GPIOV33 pins.
+ */
+
+static int __init davinci_gpio_irq_setup(void)
+{
+	unsigned	gpio, irq, bank;
+	struct clk	*clk;
+
+	clk = clk_get(NULL, "gpio");
+	if (IS_ERR(clk)) {
+		printk(KERN_ERR "Error %ld getting gpio clock?\n",
+		       PTR_ERR(clk));
+		return 0;
+	}
+
+	clk_enable(clk);
+
+	for (gpio = 0, irq = gpio_to_irq(0), bank = IRQ_GPIOBNK0;
+	     gpio < DAVINCI_N_GPIO; bank++) {
+		struct gpio_controller	*__iomem g = gpio2controller(gpio);
+		unsigned		i;
+
+		__raw_writel(~0, &g->clr_falling);
+		__raw_writel(~0, &g->clr_rising);
+
+		/* set up all irqs in this bank */
+		set_irq_chained_handler(bank, gpio_irq_handler);
+		set_irq_chip_data(bank, g);
+		set_irq_data(bank, (void *)irq);
+
+		for (i = 0; i < 16 && gpio < DAVINCI_N_GPIO;
+		     i++, irq++, gpio++) {
+			set_irq_chip(irq, &gpio_irqchip);
+			set_irq_chip_data(irq, g);
+			set_irq_handler(irq, handle_simple_irq);
+			set_irq_flags(irq, IRQF_VALID);
+		}
+	}
+
+	/* BINTEN -- per-bank interrupt enable. genirq would also let these
+	 * bits be set/cleared dynamically.
+	 */
+	__raw_writel(0x1f, (void *__iomem)
+		     IO_ADDRESS(DAVINCI_GPIO_BASE + 0x08));
+
+	printk(KERN_INFO "DaVinci: %d gpio irqs\n", irq - gpio_to_irq(0));
+
+	return 0;
+}
+
+arch_initcall(davinci_gpio_irq_setup);
diff --git a/arch/arm/mach-davinci/io.c b/arch/arm/mach-davinci/io.c
index 87fae6fb6ec..47787ff84a6 100644
--- a/arch/arm/mach-davinci/io.c
+++ b/arch/arm/mach-davinci/io.c
@@ -17,6 +17,7 @@
 #include <asm/memory.h>
 
 #include <asm/mach/map.h>
+#include <asm/arch/clock.h>
 
 extern void davinci_check_revision(void);
 
@@ -49,3 +50,8 @@ void __init davinci_map_common_io(void)
 	 */
 	davinci_check_revision();
 }
+
+void __init davinci_init_common_hw(void)
+{
+	davinci_clk_init();
+}
diff --git a/arch/arm/mach-davinci/mux.c b/arch/arm/mach-davinci/mux.c
new file mode 100644
index 00000000000..92d26bd305b
--- /dev/null
+++ b/arch/arm/mach-davinci/mux.c
@@ -0,0 +1,41 @@
+/*
+ * DaVinci pin multiplexing configurations
+ *
+ * Author: Vladimir Barinov, MontaVista Software, Inc. <source@mvista.com>
+ *
+ * 2007 (c) MontaVista Software, Inc. This file is licensed under
+ * the terms of the GNU General Public License version 2. This program
+ * is licensed "as is" without any warranty of any kind, whether express
+ * or implied.
+ */
+#include <linux/io.h>
+#include <linux/spinlock.h>
+
+#include <asm/hardware.h>
+
+#include <asm/arch/mux.h>
+
+/* System control register offsets */
+#define PINMUX0         0x00
+#define PINMUX1         0x04
+
+static DEFINE_SPINLOCK(mux_lock);
+
+void davinci_mux_peripheral(unsigned int mux, unsigned int enable)
+{
+	u32 pinmux, muxreg = PINMUX0;
+
+	if (mux >= DAVINCI_MUX_LEVEL2) {
+		muxreg = PINMUX1;
+		mux -= DAVINCI_MUX_LEVEL2;
+	}
+
+	spin_lock(&mux_lock);
+	pinmux = davinci_readl(DAVINCI_SYSTEM_MODULE_BASE + muxreg);
+	if (enable)
+		pinmux |= (1 << mux);
+	else
+		pinmux &= ~(1 << mux);
+	davinci_writel(pinmux, DAVINCI_SYSTEM_MODULE_BASE + muxreg);
+	spin_unlock(&mux_lock);
+}
diff --git a/arch/arm/mach-davinci/psc.c b/arch/arm/mach-davinci/psc.c
index e1b0050283a..1334416559a 100644
--- a/arch/arm/mach-davinci/psc.c
+++ b/arch/arm/mach-davinci/psc.c
@@ -25,39 +25,40 @@
 #include <asm/io.h>
 #include <asm/hardware.h>
 #include <asm/arch/psc.h>
+#include <asm/arch/mux.h>
 
-#define PTCMD	     __REG(0x01C41120)
-#define PDSTAT	     __REG(0x01C41200)
-#define PDCTL1	     __REG(0x01C41304)
-#define EPCPR	     __REG(0x01C41070)
-#define PTSTAT	     __REG(0x01C41128)
+/* PSC register offsets */
+#define EPCPR		0x070
+#define PTCMD		0x120
+#define PTSTAT		0x128
+#define PDSTAT		0x200
+#define PDCTL1		0x304
+#define MDSTAT		0x800
+#define MDCTL		0xA00
 
-#define MDSTAT	     IO_ADDRESS(0x01C41800)
-#define MDCTL	     IO_ADDRESS(0x01C41A00)
-
-#define PINMUX0	     __REG(0x01c40000)
-#define PINMUX1	     __REG(0x01c40004)
-#define VDD3P3V_PWDN __REG(0x01C40048)
+/* System control register offsets */
+#define VDD3P3V_PWDN	0x48
 
 static void davinci_psc_mux(unsigned int id)
 {
 	switch (id) {
 	case DAVINCI_LPSC_ATA:
-		PINMUX0 |= (1 << 17) | (1 << 16);
+		davinci_mux_peripheral(DAVINCI_MUX_HDIREN, 1);
+		davinci_mux_peripheral(DAVINCI_MUX_ATAEN, 1);
 		break;
 	case DAVINCI_LPSC_MMC_SD:
 		/* VDD power manupulations are done in U-Boot for CPMAC
 		 * so applies to MMC as well
 		 */
 		/*Set up the pull regiter for MMC */
-		VDD3P3V_PWDN = 0x0;
-		PINMUX1 &= (~(1 << 9));
+		davinci_writel(0, DAVINCI_SYSTEM_MODULE_BASE + VDD3P3V_PWDN);
+		davinci_mux_peripheral(DAVINCI_MUX_MSTK, 0);
 		break;
 	case DAVINCI_LPSC_I2C:
-		PINMUX1 |= (1 << 7);
+		davinci_mux_peripheral(DAVINCI_MUX_I2C, 1);
 		break;
 	case DAVINCI_LPSC_McBSP:
-		PINMUX1 |= (1 << 10);
+		davinci_mux_peripheral(DAVINCI_MUX_ASP, 1);
 		break;
 	default:
 		break;
@@ -67,33 +68,59 @@ static void davinci_psc_mux(unsigned int id)
 /* Enable or disable a PSC domain */
 void davinci_psc_config(unsigned int domain, unsigned int id, char enable)
 {
-	volatile unsigned int *mdstat = (unsigned int *)((int)MDSTAT + 4 * id);
-	volatile unsigned int *mdctl = (unsigned int *)((int)MDCTL + 4 * id);
+	u32 epcpr, ptcmd, ptstat, pdstat, pdctl1, mdstat, mdctl, mdstat_mask;
 
 	if (id < 0)
 		return;
 
+	mdctl = davinci_readl(DAVINCI_PWR_SLEEP_CNTRL_BASE + MDCTL + 4 * id);
 	if (enable)
-		*mdctl |= 0x00000003;	/* Enable Module */
+		mdctl |= 0x00000003;	/* Enable Module */
 	else
-		*mdctl &= 0xFFFFFFF2;	/* Disable Module */
+		mdctl &= 0xFFFFFFF2;	/* Disable Module */
+	davinci_writel(mdctl, DAVINCI_PWR_SLEEP_CNTRL_BASE + MDCTL + 4 * id);
+
+	pdstat = davinci_readl(DAVINCI_PWR_SLEEP_CNTRL_BASE + PDSTAT);
+	if ((pdstat & 0x00000001) == 0) {
+		pdctl1 = davinci_readl(DAVINCI_PWR_SLEEP_CNTRL_BASE + PDCTL1);
+		pdctl1 |= 0x1;
+		davinci_writel(pdctl1, DAVINCI_PWR_SLEEP_CNTRL_BASE + PDCTL1);
+
+		ptcmd = 1 << domain;
+		davinci_writel(ptcmd, DAVINCI_PWR_SLEEP_CNTRL_BASE + PTCMD);
 
-	if ((PDSTAT & 0x00000001) == 0) {
-		PDCTL1 |= 0x1;
-		PTCMD = (1 << domain);
-		while ((((EPCPR >> domain) & 1) == 0));
+		do {
+			epcpr = davinci_readl(DAVINCI_PWR_SLEEP_CNTRL_BASE +
+					      EPCPR);
+		} while ((((epcpr >> domain) & 1) == 0));
 
-		PDCTL1 |= 0x100;
-		while (!(((PTSTAT >> domain) & 1) == 0));
+		pdctl1 = davinci_readl(DAVINCI_PWR_SLEEP_CNTRL_BASE + PDCTL1);
+		pdctl1 |= 0x100;
+		davinci_writel(pdctl1, DAVINCI_PWR_SLEEP_CNTRL_BASE + PDCTL1);
+
+		do {
+			ptstat = davinci_readl(DAVINCI_PWR_SLEEP_CNTRL_BASE +
+					       PTSTAT);
+		} while (!(((ptstat >> domain) & 1) == 0));
 	} else {
-		PTCMD = (1 << domain);
-		while (!(((PTSTAT >> domain) & 1) == 0));
+		ptcmd = 1 << domain;
+		davinci_writel(ptcmd, DAVINCI_PWR_SLEEP_CNTRL_BASE + PTCMD);
+
+		do {
+			ptstat = davinci_readl(DAVINCI_PWR_SLEEP_CNTRL_BASE +
+					       PTSTAT);
+		} while (!(((ptstat >> domain) & 1) == 0));
 	}
 
 	if (enable)
-		while (!((*mdstat & 0x0000001F) == 0x3));
+		mdstat_mask = 0x3;
 	else
-		while (!((*mdstat & 0x0000001F) == 0x2));
+		mdstat_mask = 0x2;
+
+	do {
+		mdstat = davinci_readl(DAVINCI_PWR_SLEEP_CNTRL_BASE +
+				       MDSTAT + 4 * id);
+	} while (!((mdstat & 0x0000001F) == mdstat_mask));
 
 	if (enable)
 		davinci_psc_mux(id);