Merge branch 'for-next/at91'

16 files changed, 395 insertions, 65 deletions

diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 1cf0ccb968..dedbf6c4dd 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -10,6 +10,7 @@ config COMMON_CLK
 
 config COMMON_CLK_OF_PROVIDER
 	bool
+	depends on OFTREE
 	help
 	  Clock driver provides OF-Tree based clock lookup.
 
diff --git a/drivers/clk/clk-fixed-factor.c b/drivers/clk/clk-fixed-factor.c
index 40b63d6d50..a3dbf334a8 100644
--- a/drivers/clk/clk-fixed-factor.c
+++ b/drivers/clk/clk-fixed-factor.c
@@ -93,7 +93,7 @@ struct clk *clk_fixed_factor(const char *name,
 	return &f->clk;
 }
 
-#if defined(CONFIG_OFTREE) && defined(CONFIG_COMMON_CLK_OF_PROVIDER)
+#if defined(CONFIG_COMMON_CLK_OF_PROVIDER)
 /**
  * of_fixed_factor_clk_setup() - Setup function for simple fixed factor clock
  */
diff --git a/drivers/clk/clk-fixed.c b/drivers/clk/clk-fixed.c
index 8164005a05..f0f7fbaed5 100644
--- a/drivers/clk/clk-fixed.c
+++ b/drivers/clk/clk-fixed.c
@@ -55,7 +55,7 @@ struct clk *clk_fixed(const char *name, int rate)
 	return &fix->clk;
 }
 
-#if defined(CONFIG_OFTREE) && defined(CONFIG_COMMON_CLK_OF_PROVIDER)
+#if defined(CONFIG_COMMON_CLK_OF_PROVIDER)
 /**
  * of_fixed_clk_setup() - Setup function for simple fixed rate clock
  */
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index 93e000c6e9..5bb147eee0 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -346,7 +346,7 @@ int clk_parent_set_rate(struct clk *clk, unsigned long rate,
 	return clk_set_rate(clk_get_parent(clk), rate);
 }
 
-#if defined(CONFIG_OFTREE) && defined(CONFIG_COMMON_CLK_OF_PROVIDER)
+#if defined(CONFIG_COMMON_CLK_OF_PROVIDER)
 /**
  * struct of_clk_provider - Clock provider registration structure
  * @link: Entry in global list of clock providers
@@ -452,6 +452,24 @@ struct clk *of_clk_get_from_provider(struct of_phandle_args *clkspec)
 	return clk;
 }
 
+/**
+ * of_clk_get_parent_count() - Count the number of clocks a device node has
+ * @np: device node to count
+ *
+ * Returns: The number of clocks that are possible parents of this node
+ */
+unsigned int of_clk_get_parent_count(struct device_node *np)
+{
+	int count;
+
+	count = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	if (count < 0)
+		return 0;
+
+	return count;
+}
+EXPORT_SYMBOL_GPL(of_clk_get_parent_count);
+
 char *of_clk_get_parent_name(struct device_node *np, unsigned int index)
 {
 	struct of_phandle_args clkspec;
@@ -472,6 +490,27 @@ char *of_clk_get_parent_name(struct device_node *np, unsigned int index)
 }
 EXPORT_SYMBOL_GPL(of_clk_get_parent_name);
 
+/**
+ * of_clk_parent_fill() - Fill @parents with names of @np's parents and return
+ * number of parents
+ * @np: Device node pointer associated with clock provider
+ * @parents: pointer to char array that hold the parents' names
+ * @size: size of the @parents array
+ *
+ * Return: number of parents for the clock node.
+ */
+int of_clk_parent_fill(struct device_node *np, const char **parents,
+		       unsigned int size)
+{
+	unsigned int i = 0;
+
+	while (i < size && (parents[i] = of_clk_get_parent_name(np, i)) != NULL)
+		i++;
+
+	return i;
+}
+EXPORT_SYMBOL_GPL(of_clk_parent_fill);
+
 struct clock_provider {
 	of_clk_init_cb_t clk_init_cb;
 	struct device_node *np;
diff --git a/drivers/clk/clkdev.c b/drivers/clk/clkdev.c
index 1bc5c6daa6..7f9f8f2adc 100644
--- a/drivers/clk/clkdev.c
+++ b/drivers/clk/clkdev.c
@@ -24,7 +24,7 @@
 
 static LIST_HEAD(clocks);
 
-#if defined(CONFIG_OFTREE) && defined(CONFIG_COMMON_CLK_OF_PROVIDER)
+#if defined(CONFIG_COMMON_CLK_OF_PROVIDER)
 struct clk *of_clk_get(struct device_node *np, int index)
 {
 	struct of_phandle_args clkspec;
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index f1ab554f96..f3c3255ffc 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -53,3 +53,7 @@ config CLOCKSOURCE_UEMD
 config CLOCKSOURCE_ROCKCHIP
 	bool
 	depends on ARCH_ROCKCHIP
+
+config CLOCKSOURCE_ATMEL_PIT
+       bool
+       depends on SOC_AT91SAM9 || SOC_SAMA5
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index 39982ffb28..0564d8f5a9 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -8,3 +8,4 @@ obj-$(CONFIG_CLOCKSOURCE_NOMADIK) += nomadik.o
 obj-$(CONFIG_CLOCKSOURCE_ORION)   += orion.o
 obj-$(CONFIG_CLOCKSOURCE_UEMD)    += uemd.o
 obj-$(CONFIG_CLOCKSOURCE_ROCKCHIP)+= rk_timer.o
+obj-$(CONFIG_CLOCKSOURCE_ATMEL_PIT) += timer-atmel-pit.o
diff --git a/drivers/clocksource/timer-atmel-pit.c b/drivers/clocksource/timer-atmel-pit.c
new file mode 100644
index 0000000000..cc7ad2f39a
--- /dev/null
+++ b/drivers/clocksource/timer-atmel-pit.c
@@ -0,0 +1,114 @@
+/*
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Marius Groeger <mgroeger@sysgo.de>
+ *
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Alex Zuepke <azu@sysgo.de>
+ *
+ * (C) Copyright 2002
+ * Gary Jennejohn, DENX Software Engineering, <gj@denx.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <common.h>
+#include <init.h>
+#include <clock.h>
+#include <mach/hardware.h>
+#include <mach/at91_pit.h>
+#include <mach/io.h>
+#include <io.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+
+#define PIT_CPIV(x)	((x) & AT91_PIT_CPIV)
+#define pit_write(reg, val)	__raw_writel(val, pit_base + reg)
+#define pit_read(reg)		__raw_readl(pit_base + reg)
+
+static __iomem void *pit_base;
+
+uint64_t at91sam9_clocksource_read(void)
+{
+	return pit_read(AT91_PIT_PIIR);
+}
+
+static struct clocksource cs = {
+	.read	= at91sam9_clocksource_read,
+	.mask	= CLOCKSOURCE_MASK(32),
+	.shift	= 10,
+};
+
+static void at91_pit_stop(void)
+{
+	/* Disable timer and irqs */
+	pit_write(AT91_PIT_MR, 0);
+
+	/* Clear any pending interrupts, wait for PIT to stop counting */
+	while (PIT_CPIV(pit_read(AT91_PIT_PIVR)) != 0);
+}
+
+static void at91sam926x_pit_reset(void)
+{
+	at91_pit_stop();
+
+	/* Start PIT but don't enable IRQ */
+	pit_write(AT91_PIT_MR, 0xfffff | AT91_PIT_PITEN);
+}
+
+static int at91_pit_probe(struct device_d *dev)
+{
+	struct clk *clk;
+	u32 pit_rate;
+	int ret;
+
+	clk = clk_get(dev, NULL);
+	if (IS_ERR(clk)) {
+		ret = PTR_ERR(clk);
+		dev_err(dev, "clock not found: %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_enable(clk);
+	if (ret < 0) {
+		dev_err(dev, "clock failed to enable: %d\n", ret);
+		clk_put(clk);
+		return ret;
+	}
+
+	pit_base = dev_request_mem_region_err_null(dev, 0);
+	if (!pit_base)
+		return -ENOENT;
+
+	pit_rate = clk_get_rate(clk) / 16;
+
+	at91sam926x_pit_reset();
+
+	cs.mult = clocksource_hz2mult(pit_rate, cs.shift);
+
+	return init_clock(&cs);
+}
+
+static struct driver_d at91_pit_driver = {
+	.name = "at91-pit",
+	.probe = at91_pit_probe,
+};
+
+static int at91_pit_init(void)
+{
+	return platform_driver_register(&at91_pit_driver);
+}
+postcore_initcall(at91_pit_init);
diff --git a/drivers/mfd/syscon.c b/drivers/mfd/syscon.c
index 6ef30ce195..957d9a7267 100644
--- a/drivers/mfd/syscon.c
+++ b/drivers/mfd/syscon.c
@@ -28,6 +28,34 @@ struct syscon {
 	struct device_node *np;
 	void __iomem *base;
 	struct list_head list;
+	struct regmap *regmap;
+};
+
+static int syscon_reg_write(void *context, unsigned int reg,
+			    unsigned int val)
+{
+	struct syscon *syscon = context;
+	writel(val, syscon->base + reg);
+	return 0;
+}
+
+static int syscon_reg_read(void *context, unsigned int reg,
+			   unsigned int *val)
+{
+	struct syscon *syscon = context;
+	*val = readl(syscon->base + reg);
+	return 0;
+}
+
+static const struct regmap_bus syscon_regmap_bus = {
+	.reg_write = syscon_reg_write,
+	.reg_read  = syscon_reg_read,
+};
+
+static const struct regmap_config syscon_regmap_config = {
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_stride = 4,
 };
 
 static struct syscon *of_syscon_register(struct device_node *np)
@@ -51,6 +79,10 @@ static struct syscon *of_syscon_register(struct device_node *np)
 
 	list_add_tail(&syscon->list, &syscon_list);
 
+	syscon->regmap = regmap_init(NULL,
+				     &syscon_regmap_bus,
+				     syscon,
+				     &syscon_regmap_config);
 	return syscon;
 
 err_map:
@@ -58,7 +90,7 @@ err_map:
 	return ERR_PTR(ret);
 }
 
-static void __iomem *syscon_node_to_base(struct device_node *np)
+static struct syscon *node_to_syscon(struct device_node *np)
 {
 	struct syscon *entry, *syscon = NULL;
 
@@ -74,6 +106,16 @@ static void __iomem *syscon_node_to_base(struct device_node *np)
 	if (IS_ERR(syscon))
 		return ERR_CAST(syscon);
 
+	return syscon;
+}
+
+static void __iomem *syscon_node_to_base(struct device_node *np)
+{
+	struct syscon *syscon = node_to_syscon(np);
+
+	if (IS_ERR(syscon))
+		return ERR_CAST(syscon);
+
 	return syscon->base;
 }
 
@@ -108,6 +150,16 @@ void __iomem *syscon_base_lookup_by_phandle(struct device_node *np,
 	return syscon_node_to_base(syscon_np);
 }
 
+struct regmap *syscon_node_to_regmap(struct device_node *np)
+{
+	struct syscon *syscon = node_to_syscon(np);
+
+	if (IS_ERR(syscon))
+		return ERR_CAST(syscon);
+
+	return syscon->regmap;
+}
+
 static int syscon_probe(struct device_d *dev)
 {
 	struct syscon *syscon;
diff --git a/drivers/of/base.c b/drivers/of/base.c
index 0c20fcd5c3..bef8f1de1a 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -472,21 +472,20 @@ EXPORT_SYMBOL(of_get_cpu_node);
 int of_device_is_compatible(const struct device_node *device,
 		const char *compat)
 {
+	struct property *prop;
 	const char *cp;
-	int cplen, l;
+	int index = 0, score = 0;
 
-	cp = of_get_property(device, "compatible", &cplen);
-	if (cp == NULL)
-		return 0;
-	while (cplen > 0) {
-		if (of_compat_cmp(cp, compat, strlen(compat)) == 0)
-			return 1;
-		l = strlen(cp) + 1;
-		cp += l;
-		cplen -= l;
+	prop = of_find_property(device, "compatible", NULL);
+	for (cp = of_prop_next_string(prop, NULL); cp;
+	     cp = of_prop_next_string(prop, cp), index++) {
+		if (of_compat_cmp(cp, compat, strlen(compat)) == 0) {
+			score = INT_MAX/2 - (index << 2);
+			break;
+		}
 	}
 
-	return 0;
+	return score;
 }
 EXPORT_SYMBOL(of_device_is_compatible);
 
@@ -602,16 +601,22 @@ EXPORT_SYMBOL(of_find_node_with_property);
 const struct of_device_id *of_match_node(const struct of_device_id *matches,
 					 const struct device_node *node)
 {
+	const struct of_device_id *best_match = NULL;
+	int score, best_score = 0;
+
 	if (!matches || !node)
 		return NULL;
 
-	while (matches->compatible) {
-		if (of_device_is_compatible(node, matches->compatible) == 1)
-			return matches;
-		matches++;
+	for (; matches->compatible; matches++) {
+		score = of_device_is_compatible(node, matches->compatible);
+
+		if (score > best_score) {
+			best_match = matches;
+			best_score = score;
+		}
 	}
 
-	return NULL;
+	return best_match;
 }
 
 /**
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index ebbc6f6f14..021c1e5a2e 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -414,8 +414,10 @@ static int at91_pinctrl_set_conf(struct at91_pinctrl *info, unsigned int pin_num
 {
 	unsigned int mask;
 	void __iomem *pio;
+	struct at91_gpio_chip *at91_gpio;
 
-	pio = pin_to_controller(pin_num);
+	at91_gpio = pin_to_controller(pin_num);
+	pio  = at91_gpio->regbase;
 	mask = pin_to_mask(pin_num);
 
 	if (conf & PULL_UP && conf & PULL_DOWN)
@@ -460,6 +462,8 @@ static int at91_pinctrl_set_state(struct pinctrl_device *pdev, struct device_nod
 		mux = be32_to_cpu(*list++);
 		conf = be32_to_cpu(*list++);
 
+		pin_num += bank_num * MAX_NB_GPIO_PER_BANK;
+
 		ret = at91_mux_pin(pin_num, mux, conf & PULL_UP);
 		if (ret) {
 			dev_err(pdev->dev, "failed to mux pin %d\n", pin_num);
@@ -564,6 +568,21 @@ static int at91_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 	return 0;
 }
 
+static int at91_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
+{
+	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
+	void __iomem *pio = at91_gpio->regbase;
+	unsigned mask = 1 << offset;
+	u32 osr;
+
+	if (mask & __raw_readl(pio + PIO_PSR)) {
+		osr = __raw_readl(pio + PIO_OSR);
+		return !(osr & mask);
+	} else {
+		return -EBUSY;
+	}
+}
+
 static int at91_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
 	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
@@ -599,6 +618,7 @@ static struct gpio_ops at91_gpio_ops = {
 	.free = at91_gpio_free,
 	.direction_input = at91_gpio_direction_input,
 	.direction_output = at91_gpio_direction_output,
+	.get_direction = at91_gpio_get_direction,
 	.get = at91_gpio_get,
 	.set = at91_gpio_set,
 };
diff --git a/drivers/serial/atmel.c b/drivers/serial/atmel.c
index 4e4624e235..ab94fd2177 100644
--- a/drivers/serial/atmel.c
+++ b/drivers/serial/atmel.c
@@ -403,6 +403,11 @@ static int atmel_serial_init_port(struct console_device *cdev)
 		return -ENOENT;
 
 	uart->clk = clk_get(dev, "usart");
+	if (IS_ERR(uart->clk)) {
+		dev_err(dev, "Failed to get 'usart' clock\n");
+		return PTR_ERR(uart->clk);
+	}
+
 	clk_enable(uart->clk);
 	uart->uartclk = clk_get_rate(uart->clk);
 
diff --git a/drivers/spi/atmel_spi.c b/drivers/spi/atmel_spi.c
index 3f2c527d14..ef57867759 100644
--- a/drivers/spi/atmel_spi.c
+++ b/drivers/spi/atmel_spi.c
@@ -40,11 +40,16 @@
 
 #include "atmel_spi.h"
 
+struct atmel_spi_caps {
+	bool	is_spi2;
+};
+
 struct atmel_spi {
 	struct spi_master	master;
 	void __iomem		*regs;
 	struct clk		*clk;
 	int			*cs_pins;
+	struct atmel_spi_caps	caps;
 };
 
 #define to_atmel_spi(p)		container_of(p, struct atmel_spi, master)
@@ -62,9 +67,9 @@ struct atmel_spi {
  * register, but I haven't checked that it exists on all chips, and
  * this is cheaper anyway.
  */
-static inline bool atmel_spi_is_v2(void)
+static inline bool atmel_spi_is_v2(struct atmel_spi *as)
 {
-	return !cpu_is_at91rm9200();
+	return as->caps.is_spi2;
 }
 
 /*
@@ -104,7 +109,7 @@ static void cs_activate(struct atmel_spi *as, struct spi_device *spi)
 
 	csr = (u32)spi->controller_data;
 
-	if (atmel_spi_is_v2()) {
+	if (atmel_spi_is_v2(as)) {
 		/*
 		 * Always use CSR0. This ensures that the clock
 		 * switches to the correct idle polarity before we
@@ -163,8 +168,9 @@ static void cs_deactivate(struct atmel_spi *as, struct spi_device *spi)
 			npcs_pin, active ? " (low)" : "",
 			mr);
 
-	if (atmel_spi_is_v2() || npcs_pin != AT91_PIN_PA3)
+	if (atmel_spi_is_v2(as) || npcs_pin != AT91_PIN_PA3) {
 		gpio_set_value(npcs_pin, !active);
+	}
 }
 
 static int atmel_spi_setup(struct spi_device *spi)
@@ -172,6 +178,8 @@ static int atmel_spi_setup(struct spi_device *spi)
 	struct spi_master	*master = spi->master;
 	struct atmel_spi	*as = to_atmel_spi(master);
 
+	int			npcs_pin;
+	unsigned		active = spi->mode & SPI_CS_HIGH;
 	u32			scbr, csr;
 	unsigned int		bits = spi->bits_per_word;
 	unsigned long		bus_hz;
@@ -183,6 +191,8 @@ static int atmel_spi_setup(struct spi_device *spi)
 		return -EINVAL;
 	}
 
+	npcs_pin = as->cs_pins[spi->chip_select];
+
 	if (bits < 8 || bits > 16) {
 		dev_dbg(&spi->dev,
 				"setup: invalid bits_per_word %u (8 to 16)\n",
@@ -195,7 +205,7 @@ static int atmel_spi_setup(struct spi_device *spi)
 			spi->max_speed_hz);
 
 	bus_hz = clk_get_rate(as->clk);
-	if (!atmel_spi_is_v2())
+	if (!atmel_spi_is_v2(as))
 		bus_hz /= 2;
 
 	if (spi->max_speed_hz) {
@@ -235,7 +245,7 @@ static int atmel_spi_setup(struct spi_device *spi)
 	csr |= SPI_BF(DLYBS, 0);
 	csr |= SPI_BF(DLYBCT, 0);
 
-	/* gpio_direction_output(npcs_pin, !(spi->mode & SPI_CS_HIGH)); */
+	gpio_direction_output(npcs_pin, !active);
 	dev_dbg(master->dev,
 		"setup: %lu Hz bpw %u mode 0x%x -> csr%d %08x\n",
 		bus_hz / scbr, bits, spi->mode, spi->chip_select, csr);
@@ -244,7 +254,7 @@ static int atmel_spi_setup(struct spi_device *spi)
 
 	cs_deactivate(as, spi);
 
-	if (!atmel_spi_is_v2())
+	if (!atmel_spi_is_v2(as))
 		spi_writel(as, CSR0 + 4 * spi->chip_select, csr);
 
 	return 0;
@@ -369,6 +379,21 @@ err:
 	return ret;
 }
 
+static inline unsigned int atmel_get_version(struct atmel_spi *as)
+{
+	return spi_readl(as, VERSION) & 0x00000fff;
+}
+
+static void atmel_get_caps(struct atmel_spi *as)
+{
+	unsigned int version;
+
+	version = atmel_get_version(as);
+	dev_info(as->master.dev, "version: 0x%x\n", version);
+
+	as->caps.is_spi2 = version > 0x121;
+}
+
 static int atmel_spi_probe(struct device_d *dev)
 {
 	struct resource *iores;
@@ -405,6 +430,8 @@ static int atmel_spi_probe(struct device_d *dev)
 		return PTR_ERR(iores);
 	as->regs = IOMEM(iores->start);
 
+	atmel_get_caps(as);
+
 	for (i = 0; i < master->num_chipselect; i++) {
 		ret = gpio_request(as->cs_pins[i], dev_name(dev));
 		if (ret)
diff --git a/drivers/spi/atmel_spi.h b/drivers/spi/atmel_spi.h
index 38ce11998a..3f4d7ba124 100644
--- a/drivers/spi/atmel_spi.h
+++ b/drivers/spi/atmel_spi.h
@@ -23,6 +23,7 @@
 #define SPI_CSR1				0x0034
 #define SPI_CSR2				0x0038
 #define SPI_CSR3				0x003c
+#define SPI_VERSION				0x00fc
 #define SPI_RPR					0x0100
 #define SPI_RCR					0x0104
 #define SPI_TPR					0x0108
diff --git a/drivers/usb/host/ehci-atmel.c b/drivers/usb/host/ehci-atmel.c
index cc9636c4b7..f075b5080c 100644
--- a/drivers/usb/host/ehci-atmel.c
+++ b/drivers/usb/host/ehci-atmel.c
@@ -29,34 +29,55 @@
 
 #include "ehci.h"
 
-/* interface and function clocks; sometimes also an AHB clock */
-static struct clk *iclk, *fclk;
+struct atmel_ehci_priv {
+	struct device_d *dev;
+	struct clk *iclk;
+	struct clk *uclk;
+};
 
-static void atmel_start_clock(void)
+static int atmel_start_clock(struct atmel_ehci_priv *atehci)
 {
-	clk_enable(iclk);
-	clk_enable(fclk);
+	int ret;
+	ret = clk_enable(atehci->iclk);
+	if (ret < 0) {
+		dev_err(atehci->dev,
+			"Error enabling interface clock\n");
+		return ret;
+	}
+
+	ret = clk_enable(atehci->uclk);
+	if (ret < 0)
+		dev_err(atehci->dev,
+			"Error enabling function clock\n");
+
+	return ret;
 }
 
-static void atmel_stop_clock(void)
+static void atmel_stop_clock(struct atmel_ehci_priv *atehci)
 {
-	clk_disable(fclk);
-	clk_disable(iclk);
+	clk_disable(atehci->iclk);
+	clk_disable(atehci->uclk);
 }
 
 static int atmel_ehci_probe(struct device_d *dev)
 {
+	int ret;
 	struct resource *iores;
 	struct ehci_data data;
+	struct atmel_ehci_priv *atehci;
+
+	atehci = xzalloc(sizeof(*atehci));
+	atehci->dev = dev;
+	dev->priv = atehci;
 
-	iclk = clk_get(dev, "ehci_clk");
-	if (IS_ERR(iclk)) {
+	atehci->iclk = clk_get(dev, "ehci_clk");
+	if (IS_ERR(atehci->iclk)) {
 		dev_err(dev, "Error getting interface clock\n");
 		return -ENOENT;
 	}
 
-	fclk = clk_get(dev, "uhpck");
-	if (IS_ERR(fclk)) {
+	atehci->uclk = clk_get(dev, "uhpck");
+	if (IS_ERR(atehci->iclk)) {
 		dev_err(dev, "Error getting function clock\n");
 		return -ENOENT;
 	}
@@ -64,18 +85,18 @@ static int atmel_ehci_probe(struct device_d *dev)
 	/*
 	 * Start the USB clocks.
 	 */
-	atmel_start_clock();
+	ret = atmel_start_clock(atehci);
+	if (ret < 0)
+		return ret;
 
-	data.flags = 0;
+	memset(&data, 0, sizeof(data));
 
 	iores = dev_request_mem_resource(dev, 0);
 	if (IS_ERR(iores))
 		return PTR_ERR(iores);
 	data.hccr = IOMEM(iores->start);
 
-	ehci_register(dev, &data);
-
-	return 0;
+	return ehci_register(dev, &data);
 }
 
 static void atmel_ehci_remove(struct device_d *dev)
@@ -83,7 +104,7 @@ static void atmel_ehci_remove(struct device_d *dev)
 	/*
 	 * Stop the USB clocks.
 	 */
-	atmel_stop_clock();
+	atmel_stop_clock(dev->priv);
 }
 
 static struct driver_d atmel_ehci_driver = {
diff --git a/drivers/usb/host/ohci-at91.c b/drivers/usb/host/ohci-at91.c
index 0f5c8f1307..5f745264b7 100644
--- a/drivers/usb/host/ohci-at91.c
+++ b/drivers/usb/host/ohci-at91.c
@@ -27,57 +27,97 @@
 
 #include "ohci.h"
 
-/* interface and function clocks; sometimes also an AHB clock */
-static struct clk *iclk, *fclk;
+struct ohci_at91_priv {
+	struct device_d *dev;
+	struct clk *iclk;
+	struct clk *fclk;
+	struct ohci_regs __iomem *regs;
+};
 
-static void at91_start_clock(void)
+static int at91_start_clock(struct ohci_at91_priv *ohci_at91)
 {
-	clk_enable(iclk);
-	clk_enable(fclk);
+	int ret;
+
+	ret = clk_enable(ohci_at91->iclk);
+	if (ret < 0) {
+		dev_err(ohci_at91->dev, "Failed to enable 'iclk'\n");
+		return ret;
+	}
+
+	ret = clk_enable(ohci_at91->fclk);
+	if (ret < 0) {
+		dev_err(ohci_at91->dev, "Failed to enable 'fclk'\n");
+		return ret;
+	}
+
+	return 0;
 }
 
-static void at91_stop_clock(void)
+static void at91_stop_clock(struct ohci_at91_priv *ohci_at91)
 {
-	clk_disable(fclk);
-	clk_disable(iclk);
+	clk_disable(ohci_at91->fclk);
+	clk_disable(ohci_at91->iclk);
 }
 
 static int at91_ohci_probe(struct device_d *dev)
 {
-	struct ohci_regs __iomem *regs = (struct ohci_regs __iomem *)dev->resource[0].start;
+	int ret;
+	struct resource *io;
+	struct ohci_at91_priv *ohci_at91 = xzalloc(sizeof(*ohci_at91));
+
+	dev->priv = ohci_at91;
+	ohci_at91->dev = dev;
+
+	io = dev_get_resource(dev, IORESOURCE_MEM, 0);
+	if (IS_ERR(io)) {
+		dev_err(dev, "Failed to get IORESOURCE_MEM\n");
+		return PTR_ERR(io);
+	}
+	ohci_at91->regs = IOMEM(io->start);
+
+	ohci_at91->iclk = clk_get(NULL, "ohci_clk");
+	if (IS_ERR(ohci_at91->iclk)) {
+		dev_err(dev, "Failed to get 'ohci_clk'\n");
+		return PTR_ERR(ohci_at91->iclk);
+	}
 
-	iclk = clk_get(NULL, "ohci_clk");
-	fclk = clk_get(NULL, "uhpck");
+	ohci_at91->fclk = clk_get(NULL, "uhpck");
+	if (IS_ERR(ohci_at91->fclk)) {
+		dev_err(dev, "Failed to get 'uhpck'\n");
+		return PTR_ERR(ohci_at91->fclk);
+	}
 
 	/*
 	 * Start the USB clocks.
 	 */
-	at91_start_clock();
+	ret = at91_start_clock(ohci_at91);
+	if (ret < 0)
+		return ret;
 
 	/*
 	 * The USB host controller must remain in reset.
 	 */
-	writel(0, &regs->control);
+	writel(0, &ohci_at91->regs->control);
 
-	add_generic_device("ohci", DEVICE_ID_DYNAMIC, NULL, dev->resource[0].start,
-			   resource_size(&dev->resource[0]), IORESOURCE_MEM, NULL);
+	add_generic_device("ohci", DEVICE_ID_DYNAMIC, NULL, io->start,
+			   resource_size(io), IORESOURCE_MEM, NULL);
 
 	return 0;
 }
 
 static void at91_ohci_remove(struct device_d *dev)
 {
-	struct ohci_regs __iomem *regs = (struct ohci_regs __iomem *)dev->resource[0].start;
+	struct ohci_at91_priv *ohci_at91 = dev->priv;
 
 	/*
 	 * Put the USB host controller into reset.
 	 */
-	writel(0, &regs->control);
+	writel(0, &ohci_at91->regs->control);
 
 	/*
 	 * Stop the USB clocks.
 	 */
-	at91_stop_clock();
+	at91_stop_clock(ohci_at91);
 }
 
 static struct driver_d at91_ohci_driver = {