Merge branch 'master' of git://git.denx.de/u-boot-atmel

31 files changed, 1794 insertions, 74 deletions

diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 7dd5673..8f3b96a 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -23,5 +23,6 @@ config SPL_CLK
 source "drivers/clk/tegra/Kconfig"
 source "drivers/clk/uniphier/Kconfig"
 source "drivers/clk/exynos/Kconfig"
+source "drivers/clk/at91/Kconfig"
 
 endmenu
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index 463b1d6..778d748 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -14,3 +14,4 @@ obj-$(CONFIG_MACH_PIC32) += clk_pic32.o
 obj-y += tegra/
 obj-$(CONFIG_CLK_UNIPHIER) += uniphier/
 obj-$(CONFIG_CLK_EXYNOS) += exynos/
+obj-$(CONFIG_CLK_AT91) += at91/
diff --git a/drivers/clk/at91/Kconfig b/drivers/clk/at91/Kconfig
new file mode 100644
index 0000000..10050d8
--- /dev/null
+++ b/drivers/clk/at91/Kconfig
@@ -0,0 +1,43 @@
+config CLK_AT91
+	bool "AT91 clock drivers"
+	depends on CLK
+	help
+	  This option is used to enable the AT91 clock driver.
+	  The driver supports the AT91 clock generator, including
+	  the oscillators and PLLs, such as main clock, slow clock,
+	  PLLA, UTMI PLL. Clocks can also be a source clock of other
+	  clocks a tree structure, such as master clock, usb device
+	  clock, matrix clock and generic clock.
+	  Devices can use a common clock API to request a particular
+	  clock, enable it and get its rate.
+
+config AT91_UTMI
+	bool "Support UTMI PLL Clock"
+	depends on CLK_AT91
+	help
+	  This option is used to enable the AT91 UTMI PLL clock
+	  driver. It is the clock provider of USB, and UPLLCK is the
+	  output of 480 MHz UTMI PLL, The souce clock of the UTMI
+	  PLL is the main clock, so the main clock must select the
+	  fast crystal oscillator to meet the frequency accuracy
+	  required by USB.
+
+config AT91_H32MX
+	bool "Support H32MX 32-bit Matrix Clock"
+	depends on CLK_AT91
+	help
+	  This option is used to enable the AT91 H32MX matrixes
+	  clock driver. There are H64MX and H32MX matrixes clocks,
+	  H64MX 64-bit matrix clocks are MCK. The H32MX 32-bit
+	  matrix clock is to be configured as MCK if MCK does not
+	  exceed 83 MHz, else it is to be configured as MCK/2.
+
+config AT91_GENERIC_CLK
+	bool "Support Generic Clock"
+	depends on CLK_AT91
+	help
+	  This option is used to enable the AT91 generic clock
+	  driver. Some peripherals may need a second clock source
+	  that may be different from the system clock. This second
+	  clock is the generic clock (GCLK) and is managed by
+	  the PMC via PMC_PCR register.
diff --git a/drivers/clk/at91/Makefile b/drivers/clk/at91/Makefile
new file mode 100644
index 0000000..fbe3cb6
--- /dev/null
+++ b/drivers/clk/at91/Makefile
@@ -0,0 +1,11 @@
+#
+# Makefile for at91 specific clk
+#
+
+obj-y += pmc.o sckc.o
+obj-y += clk-slow.o clk-main.o clk-plla.o clk-master.o
+obj-y += clk-system.o clk-peripheral.o
+
+obj-$(CONFIG_AT91_UTMI)		+= clk-utmi.o
+obj-$(CONFIG_AT91_H32MX)	+= clk-h32mx.o
+obj-$(CONFIG_AT91_GENERIC_CLK)	+= clk-generated.o
diff --git a/drivers/clk/at91/clk-generated.c b/drivers/clk/at91/clk-generated.c
new file mode 100644
index 0000000..f6164cc
--- /dev/null
+++ b/drivers/clk/at91/clk-generated.c
@@ -0,0 +1,162 @@
+/*
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm/device.h>
+#include <linux/io.h>
+#include <mach/at91_pmc.h>
+#include "pmc.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define GENERATED_SOURCE_MAX	6
+#define GENERATED_MAX_DIV	255
+
+struct generated_clk_priv {
+	u32 num_parents;
+};
+
+static ulong generated_clk_get_rate(struct clk *clk)
+{
+	struct pmc_platdata *plat = dev_get_platdata(clk->dev);
+	struct at91_pmc *pmc = plat->reg_base;
+	struct clk parent;
+	u32 tmp, gckdiv;
+	u8 parent_id;
+	int ret;
+
+	writel(clk->id & AT91_PMC_PCR_PID_MASK, &pmc->pcr);
+	tmp = readl(&pmc->pcr);
+	parent_id = (tmp >> AT91_PMC_PCR_GCKCSS_OFFSET) &
+		    AT91_PMC_PCR_GCKCSS_MASK;
+	gckdiv = (tmp >> AT91_PMC_PCR_GCKDIV_OFFSET) & AT91_PMC_PCR_GCKDIV_MASK;
+
+	ret = clk_get_by_index(clk->dev, parent_id, &parent);
+	if (ret)
+		return 0;
+
+	return clk_get_rate(&parent) / (gckdiv + 1);
+}
+
+static ulong generated_clk_set_rate(struct clk *clk, ulong rate)
+{
+	struct pmc_platdata *plat = dev_get_platdata(clk->dev);
+	struct at91_pmc *pmc = plat->reg_base;
+	struct generated_clk_priv *priv = dev_get_priv(clk->dev);
+	struct clk parent, best_parent;
+	ulong tmp_rate, best_rate = rate, parent_rate;
+	int tmp_diff, best_diff = -1;
+	u32 div, best_div = 0;
+	u8 best_parent_id = 0;
+	u8 i;
+	u32 tmp;
+	int ret;
+
+	for (i = 0; i < priv->num_parents; i++) {
+		ret = clk_get_by_index(clk->dev, i, &parent);
+		if (ret)
+			return ret;
+
+		parent_rate = clk_get_rate(&parent);
+		if (IS_ERR_VALUE(parent_rate))
+			return parent_rate;
+
+		for (div = 1; div < GENERATED_MAX_DIV + 2; div++) {
+			tmp_rate = DIV_ROUND_CLOSEST(parent_rate, div);
+			if (rate < tmp_rate)
+				continue;
+			tmp_diff = rate - tmp_rate;
+
+			if (best_diff < 0 || best_diff > tmp_diff) {
+				best_rate = tmp_rate;
+				best_diff = tmp_diff;
+
+				best_div = div - 1;
+				best_parent = parent;
+				best_parent_id = i;
+			}
+
+			if (!best_diff || tmp_rate < rate)
+				break;
+		}
+
+		if (!best_diff)
+			break;
+	}
+
+	debug("GCK: best parent: %s, best_rate = %ld, best_div = %d\n",
+	      best_parent.dev->name, best_rate, best_div);
+
+	ret = clk_enable(&best_parent);
+	if (ret)
+		return ret;
+
+	writel(clk->id & AT91_PMC_PCR_PID_MASK, &pmc->pcr);
+	tmp = readl(&pmc->pcr);
+	tmp &= ~(AT91_PMC_PCR_GCKDIV | AT91_PMC_PCR_GCKCSS);
+	tmp |= AT91_PMC_PCR_GCKCSS_(best_parent_id) |
+	       AT91_PMC_PCR_CMD_WRITE |
+	       AT91_PMC_PCR_GCKDIV_(best_div) |
+	       AT91_PMC_PCR_GCKEN;
+	writel(tmp, &pmc->pcr);
+
+	while (!(readl(&pmc->sr) & AT91_PMC_GCKRDY))
+		;
+
+	return 0;
+}
+
+static struct clk_ops generated_clk_ops = {
+	.get_rate = generated_clk_get_rate,
+	.set_rate = generated_clk_set_rate,
+};
+
+static int generated_clk_ofdata_to_platdata(struct udevice *dev)
+{
+	struct generated_clk_priv *priv = dev_get_priv(dev);
+	u32 cells[GENERATED_SOURCE_MAX];
+	u32 num_parents;
+
+	num_parents = fdtdec_get_int_array_count(gd->fdt_blob, dev->of_offset,
+						 "clocks", cells,
+						 GENERATED_SOURCE_MAX);
+
+	if (!num_parents)
+		return -1;
+
+	priv->num_parents = num_parents;
+
+	return 0;
+}
+
+static int generated_clk_bind(struct udevice *dev)
+{
+	return at91_pmc_clk_node_bind(dev);
+}
+
+static int generated_clk_probe(struct udevice *dev)
+{
+	return at91_pmc_core_probe(dev);
+}
+
+static const struct udevice_id generated_clk_match[] = {
+	{ .compatible = "atmel,sama5d2-clk-generated" },
+	{}
+};
+
+U_BOOT_DRIVER(generated_clk) = {
+	.name = "generated-clk",
+	.id = UCLASS_CLK,
+	.of_match = generated_clk_match,
+	.bind = generated_clk_bind,
+	.probe = generated_clk_probe,
+	.ofdata_to_platdata = generated_clk_ofdata_to_platdata,
+	.priv_auto_alloc_size = sizeof(struct generated_clk_priv),
+	.platdata_auto_alloc_size = sizeof(struct pmc_platdata),
+	.ops = &generated_clk_ops,
+};
diff --git a/drivers/clk/at91/clk-h32mx.c b/drivers/clk/at91/clk-h32mx.c
new file mode 100644
index 0000000..1a304ba
--- /dev/null
+++ b/drivers/clk/at91/clk-h32mx.c
@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm/device.h>
+#include <dm/util.h>
+#include <linux/io.h>
+#include <mach/at91_pmc.h>
+#include "pmc.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define H32MX_MAX_FREQ	90000000
+
+static ulong sama5d4_h32mx_clk_get_rate(struct clk *clk)
+{
+	struct pmc_platdata *plat = dev_get_platdata(clk->dev);
+	struct at91_pmc *pmc = plat->reg_base;
+	ulong rate = gd->arch.mck_rate_hz;
+
+	if (readl(&pmc->mckr) & AT91_PMC_MCKR_H32MXDIV)
+		rate /= 2;
+
+	if (rate > H32MX_MAX_FREQ)
+		dm_warn("H32MX clock is too fast\n");
+
+	return rate;
+}
+
+static struct clk_ops sama5d4_h32mx_clk_ops = {
+	.get_rate = sama5d4_h32mx_clk_get_rate,
+};
+
+static int sama5d4_h32mx_clk_probe(struct udevice *dev)
+{
+	return at91_pmc_core_probe(dev);
+}
+
+static const struct udevice_id sama5d4_h32mx_clk_match[] = {
+	{ .compatible = "atmel,sama5d4-clk-h32mx" },
+	{}
+};
+
+U_BOOT_DRIVER(sama5d4_h32mx_clk) = {
+	.name = "sama5d4-h32mx-clk",
+	.id = UCLASS_CLK,
+	.of_match = sama5d4_h32mx_clk_match,
+	.probe = sama5d4_h32mx_clk_probe,
+	.platdata_auto_alloc_size = sizeof(struct pmc_platdata),
+	.ops = &sama5d4_h32mx_clk_ops,
+};
diff --git a/drivers/clk/at91/clk-main.c b/drivers/clk/at91/clk-main.c
new file mode 100644
index 0000000..252d076
--- /dev/null
+++ b/drivers/clk/at91/clk-main.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm/device.h>
+#include <linux/io.h>
+#include <mach/at91_pmc.h>
+#include "pmc.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static int main_osc_clk_enable(struct clk *clk)
+{
+	struct pmc_platdata *plat = dev_get_platdata(clk->dev);
+	struct at91_pmc *pmc = plat->reg_base;
+
+	if (readl(&pmc->sr) & AT91_PMC_MOSCSELS)
+		return 0;
+
+	return -EINVAL;
+}
+
+static ulong main_osc_clk_get_rate(struct clk *clk)
+{
+	return gd->arch.main_clk_rate_hz;
+}
+
+static struct clk_ops main_osc_clk_ops = {
+	.enable = main_osc_clk_enable,
+	.get_rate = main_osc_clk_get_rate,
+};
+
+static int main_osc_clk_probe(struct udevice *dev)
+{
+	return at91_pmc_core_probe(dev);
+}
+
+static const struct udevice_id main_osc_clk_match[] = {
+	{ .compatible = "atmel,at91sam9x5-clk-main" },
+	{}
+};
+
+U_BOOT_DRIVER(at91sam9x5_main_osc_clk) = {
+	.name = "at91sam9x5-main-osc-clk",
+	.id = UCLASS_CLK,
+	.of_match = main_osc_clk_match,
+	.probe = main_osc_clk_probe,
+	.platdata_auto_alloc_size = sizeof(struct pmc_platdata),
+	.ops = &main_osc_clk_ops,
+};
diff --git a/drivers/clk/at91/clk-master.c b/drivers/clk/at91/clk-master.c
new file mode 100644
index 0000000..284b248
--- /dev/null
+++ b/drivers/clk/at91/clk-master.c
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm/device.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static ulong at91_master_clk_get_rate(struct clk *clk)
+{
+	return gd->arch.mck_rate_hz;
+}
+
+static struct clk_ops at91_master_clk_ops = {
+	.get_rate = at91_master_clk_get_rate,
+};
+
+static const struct udevice_id at91_master_clk_match[] = {
+	{ .compatible = "atmel,at91sam9x5-clk-master" },
+	{}
+};
+
+U_BOOT_DRIVER(at91_master_clk) = {
+	.name = "at91-master-clk",
+	.id = UCLASS_CLK,
+	.of_match = at91_master_clk_match,
+	.ops = &at91_master_clk_ops,
+};
diff --git a/drivers/clk/at91/clk-peripheral.c b/drivers/clk/at91/clk-peripheral.c
new file mode 100644
index 0000000..16688e9
--- /dev/null
+++ b/drivers/clk/at91/clk-peripheral.c
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm/device.h>
+#include <linux/io.h>
+#include <mach/at91_pmc.h>
+#include "pmc.h"
+
+#define PERIPHERAL_ID_MIN	2
+#define PERIPHERAL_ID_MAX	31
+#define PERIPHERAL_MASK(id)	(1 << ((id) & PERIPHERAL_ID_MAX))
+
+static int sam9x5_periph_clk_enable(struct clk *clk)
+{
+	struct pmc_platdata *plat = dev_get_platdata(clk->dev);
+	struct at91_pmc *pmc = plat->reg_base;
+
+	if (clk->id < PERIPHERAL_ID_MIN)
+		return -1;
+
+	writel(clk->id & AT91_PMC_PCR_PID_MASK, &pmc->pcr);
+	setbits_le32(&pmc->pcr, AT91_PMC_PCR_CMD_WRITE | AT91_PMC_PCR_EN);
+
+	return 0;
+}
+
+static struct clk_ops sam9x5_periph_clk_ops = {
+	.enable = sam9x5_periph_clk_enable,
+};
+
+static int sam9x5_periph_clk_bind(struct udevice *dev)
+{
+	return at91_pmc_clk_node_bind(dev);
+}
+
+static int sam9x5_periph_clk_probe(struct udevice *dev)
+{
+	return at91_pmc_core_probe(dev);
+}
+
+static const struct udevice_id sam9x5_periph_clk_match[] = {
+	{ .compatible = "atmel,at91sam9x5-clk-peripheral" },
+	{}
+};
+
+U_BOOT_DRIVER(sam9x5_periph_clk) = {
+	.name = "sam9x5-periph-clk",
+	.id = UCLASS_CLK,
+	.of_match = sam9x5_periph_clk_match,
+	.bind = sam9x5_periph_clk_bind,
+	.probe = sam9x5_periph_clk_probe,
+	.platdata_auto_alloc_size = sizeof(struct pmc_platdata),
+	.ops = &sam9x5_periph_clk_ops,
+};
diff --git a/drivers/clk/at91/clk-plla.c b/drivers/clk/at91/clk-plla.c
new file mode 100644
index 0000000..2a71399
--- /dev/null
+++ b/drivers/clk/at91/clk-plla.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm/device.h>
+#include <linux/io.h>
+#include <mach/at91_pmc.h>
+#include "pmc.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static int plla_clk_enable(struct clk *clk)
+{
+	struct pmc_platdata *plat = dev_get_platdata(clk->dev);
+	struct at91_pmc *pmc = plat->reg_base;
+
+	if (readl(&pmc->sr) & AT91_PMC_LOCKA)
+		return 0;
+
+	return -EINVAL;
+}
+
+static ulong plla_clk_get_rate(struct clk *clk)
+{
+	return gd->arch.plla_rate_hz;
+}
+
+static struct clk_ops plla_clk_ops = {
+	.enable = plla_clk_enable,
+	.get_rate = plla_clk_get_rate,
+};
+
+static int plla_clk_probe(struct udevice *dev)
+{
+	return at91_pmc_core_probe(dev);
+}
+
+static const struct udevice_id plla_clk_match[] = {
+	{ .compatible = "atmel,sama5d3-clk-pll" },
+	{}
+};
+
+U_BOOT_DRIVER(at91_plla_clk) = {
+	.name = "at91-plla-clk",
+	.id = UCLASS_CLK,
+	.of_match = plla_clk_match,
+	.probe = plla_clk_probe,
+	.platdata_auto_alloc_size = sizeof(struct pmc_platdata),
+	.ops = &plla_clk_ops,
+};
diff --git a/drivers/clk/at91/clk-slow.c b/drivers/clk/at91/clk-slow.c
new file mode 100644
index 0000000..f7666b4
--- /dev/null
+++ b/drivers/clk/at91/clk-slow.c
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm/device.h>
+
+static int at91_slow_clk_enable(struct clk *clk)
+{
+	return 0;
+}
+
+static ulong at91_slow_clk_get_rate(struct clk *clk)
+{
+	return CONFIG_SYS_AT91_SLOW_CLOCK;
+}
+
+static struct clk_ops at91_slow_clk_ops = {
+	.enable = at91_slow_clk_enable,
+	.get_rate = at91_slow_clk_get_rate,
+};
+
+static const struct udevice_id at91_slow_clk_match[] = {
+	{ .compatible = "atmel,at91sam9x5-clk-slow" },
+	{}
+};
+
+U_BOOT_DRIVER(at91_slow_clk) = {
+	.name = "at91-slow-clk",
+	.id = UCLASS_CLK,
+	.of_match = at91_slow_clk_match,
+	.ops = &at91_slow_clk_ops,
+};
diff --git a/drivers/clk/at91/clk-system.c b/drivers/clk/at91/clk-system.c
new file mode 100644
index 0000000..fa80bad
--- /dev/null
+++ b/drivers/clk/at91/clk-system.c
@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm/device.h>
+#include <linux/io.h>
+#include <mach/at91_pmc.h>
+#include "pmc.h"
+
+#define SYSTEM_MAX_ID		31
+
+static inline int is_pck(int id)
+{
+	return (id >= 8) && (id <= 15);
+}
+
+static int at91_system_clk_enable(struct clk *clk)
+{
+	struct pmc_platdata *plat = dev_get_platdata(clk->dev);
+	struct at91_pmc *pmc = plat->reg_base;
+	u32 mask;
+
+	if (clk->id > SYSTEM_MAX_ID)
+		return -EINVAL;
+
+	mask = BIT(clk->id);
+
+	writel(mask, &pmc->scer);
+
+	/**
+	 * For the programmable clocks the Ready status in the PMC
+	 * status register should be checked after enabling.
+	 * For other clocks this is unnecessary.
+	 */
+	if (!is_pck(clk->id))
+		return 0;
+
+	while (!(readl(&pmc->sr) & mask))
+		;
+
+	return 0;
+}
+
+static struct clk_ops at91_system_clk_ops = {
+	.enable = at91_system_clk_enable,
+};
+
+static int at91_system_clk_bind(struct udevice *dev)
+{
+	return at91_pmc_clk_node_bind(dev);
+}
+
+static int at91_system_clk_probe(struct udevice *dev)
+{
+	return at91_pmc_core_probe(dev);
+}
+
+static const struct udevice_id at91_system_clk_match[] = {
+	{ .compatible = "atmel,at91rm9200-clk-system" },
+	{}
+};
+
+U_BOOT_DRIVER(at91_system_clk) = {
+	.name = "at91-system-clk",
+	.id = UCLASS_CLK,
+	.of_match = at91_system_clk_match,
+	.bind = at91_system_clk_bind,
+	.probe = at91_system_clk_probe,
+	.platdata_auto_alloc_size = sizeof(struct pmc_platdata),
+	.ops = &at91_system_clk_ops,
+};
diff --git a/drivers/clk/at91/clk-utmi.c b/drivers/clk/at91/clk-utmi.c
new file mode 100644
index 0000000..369a687
--- /dev/null
+++ b/drivers/clk/at91/clk-utmi.c
@@ -0,0 +1,67 @@
+/*
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm/device.h>
+#include <linux/io.h>
+#include <mach/at91_pmc.h>
+#include "pmc.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define UTMI_FIXED_MUL		40
+
+static int utmi_clk_enable(struct clk *clk)
+{
+	struct pmc_platdata *plat = dev_get_platdata(clk->dev);
+	struct at91_pmc *pmc = plat->reg_base;
+	u32 tmp;
+
+	if (readl(&pmc->sr) & AT91_PMC_LOCKU)
+		return 0;
+
+	tmp = readl(&pmc->uckr);
+	tmp |= AT91_PMC_UPLLEN |
+	       AT91_PMC_UPLLCOUNT |
+	       AT91_PMC_BIASEN;
+	writel(tmp, &pmc->uckr);
+
+	while (!(readl(&pmc->sr) & AT91_PMC_LOCKU))
+		;
+
+	return 0;
+}
+
+static ulong utmi_clk_get_rate(struct clk *clk)
+{
+	return gd->arch.main_clk_rate_hz * UTMI_FIXED_MUL;
+}
+
+static struct clk_ops utmi_clk_ops = {
+	.enable = utmi_clk_enable,
+	.get_rate = utmi_clk_get_rate,
+};
+
+static int utmi_clk_probe(struct udevice *dev)
+{
+	return at91_pmc_core_probe(dev);
+}
+
+static const struct udevice_id utmi_clk_match[] = {
+	{ .compatible = "atmel,at91sam9x5-clk-utmi" },
+	{}
+};
+
+U_BOOT_DRIVER(at91sam9x5_utmi_clk) = {
+	.name = "at91sam9x5-utmi-clk",
+	.id = UCLASS_CLK,
+	.of_match = utmi_clk_match,
+	.probe = utmi_clk_probe,
+	.platdata_auto_alloc_size = sizeof(struct pmc_platdata),
+	.ops = &utmi_clk_ops,
+};
diff --git a/drivers/clk/at91/pmc.c b/drivers/clk/at91/pmc.c
new file mode 100644
index 0000000..a08d7e8
--- /dev/null
+++ b/drivers/clk/at91/pmc.c
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm/device.h>
+#include <dm/lists.h>
+#include <dm/root.h>
+#include "pmc.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static int at91_pmc_bind(struct udevice *dev)
+{
+	return dm_scan_fdt_node(dev, gd->fdt_blob, dev->of_offset, false);
+}
+
+static const struct udevice_id at91_pmc_match[] = {
+	{ .compatible = "atmel,sama5d2-pmc" },
+	{}
+};
+
+U_BOOT_DRIVER(at91_pmc) = {
+	.name = "at91-pmc-core",
+	.id = UCLASS_CLK,
+	.of_match = at91_pmc_match,
+	.bind = at91_pmc_bind,
+};
+
+int at91_pmc_core_probe(struct udevice *dev)
+{
+	struct pmc_platdata *plat = dev_get_platdata(dev);
+
+	dev = dev_get_parent(dev);
+
+	plat->reg_base = (struct at91_pmc *)dev_get_addr_ptr(dev);
+
+	return 0;
+}
+
+int at91_pmc_clk_node_bind(struct udevice *dev)
+{
+	const void *fdt = gd->fdt_blob;
+	int offset = dev->of_offset;
+	const char *name;
+	int ret;
+
+	for (offset = fdt_first_subnode(fdt, offset);
+	     offset > 0;
+	     offset = fdt_next_subnode(fdt, offset)) {
+		name = fdt_get_name(fdt, offset, NULL);
+		if (!name)
+			return -EINVAL;
+
+		ret = device_bind_driver_to_node(dev, "clk", name,
+						 offset, NULL);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+U_BOOT_DRIVER(clk_generic) = {
+	.id	= UCLASS_CLK,
+	.name	= "clk",
+};
diff --git a/drivers/clk/at91/pmc.h b/drivers/clk/at91/pmc.h
new file mode 100644
index 0000000..5444c84
--- /dev/null
+++ b/drivers/clk/at91/pmc.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef __AT91_PMC_H__
+#define __AT91_PMC_H__
+
+struct pmc_platdata {
+	struct at91_pmc *reg_base;
+};
+
+int at91_pmc_core_probe(struct udevice *dev);
+int at91_pmc_clk_node_bind(struct udevice *dev);
+
+#endif
diff --git a/drivers/clk/at91/sckc.c b/drivers/clk/at91/sckc.c
new file mode 100644
index 0000000..b207611
--- /dev/null
+++ b/drivers/clk/at91/sckc.c
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm/device.h>
+#include <dm/root.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static int at91_sckc_clk_bind(struct udevice *dev)
+{
+	return dm_scan_fdt_node(dev, gd->fdt_blob, dev->of_offset, false);
+}
+
+static const struct udevice_id at91_sckc_clk_match[] = {
+	{ .compatible = "atmel,at91sam9x5-sckc" },
+	{}
+};
+
+U_BOOT_DRIVER(at91_sckc_clk) = {
+	.name = "at91_sckc_clk",
+	.id = UCLASS_CLK,
+	.of_match = at91_sckc_clk_match,
+	.bind = at91_sckc_clk_bind,
+};
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 64cf221..8d9ab52 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -30,7 +30,7 @@ config DWAPB_GPIO
 
 config ATMEL_PIO4
 	bool "ATMEL PIO4 driver"
-	depends on DM
+	depends on DM_GPIO
 	default n
 	help
 	  Say yes here to support the Atmel PIO4 driver.
diff --git a/drivers/gpio/atmel_pio4.c b/drivers/gpio/atmel_pio4.c
index d71f525..7adea88 100644
--- a/drivers/gpio/atmel_pio4.c
+++ b/drivers/gpio/atmel_pio4.c
@@ -7,45 +7,16 @@
  * SPDX-License-Identifier:	GPL-2.0+
  */
 #include <common.h>
+#include <clk.h>
 #include <dm.h>
+#include <fdtdec.h>
+#include <dm/root.h>
 #include <asm/arch/hardware.h>
+#include <asm/gpio.h>
 #include <mach/gpio.h>
 #include <mach/atmel_pio4.h>
 
-#define ATMEL_PIO4_PINS_PER_BANK	32
-
-/*
- * Register Field Definitions
- */
-#define ATMEL_PIO4_CFGR_FUNC	(0x7 << 0)
-#define		ATMEL_PIO4_CFGR_FUNC_GPIO	(0x0 << 0)
-#define		ATMEL_PIO4_CFGR_FUNC_PERIPH_A	(0x1 << 0)
-#define		ATMEL_PIO4_CFGR_FUNC_PERIPH_B	(0x2 << 0)
-#define		ATMEL_PIO4_CFGR_FUNC_PERIPH_C	(0x3 << 0)
-#define		ATMEL_PIO4_CFGR_FUNC_PERIPH_D	(0x4 << 0)
-#define		ATMEL_PIO4_CFGR_FUNC_PERIPH_E	(0x5 << 0)
-#define		ATMEL_PIO4_CFGR_FUNC_PERIPH_F	(0x6 << 0)
-#define		ATMEL_PIO4_CFGR_FUNC_PERIPH_G	(0x7 << 0)
-#define ATMEL_PIO4_CFGR_DIR	(0x1 << 8)
-#define ATMEL_PIO4_CFGR_PUEN	(0x1 << 9)
-#define ATMEL_PIO4_CFGR_PDEN	(0x1 << 10)
-#define ATMEL_PIO4_CFGR_IFEN	(0x1 << 12)
-#define ATMEL_PIO4_CFGR_IFSCEN	(0x1 << 13)
-#define ATMEL_PIO4_CFGR_OPD	(0x1 << 14)
-#define ATMEL_PIO4_CFGR_SCHMITT	(0x1 << 15)
-#define ATMEL_PIO4_CFGR_DRVSTR	(0x3 << 16)
-#define		ATMEL_PIO4_CFGR_DRVSTR_LOW0	(0x0 << 16)
-#define		ATMEL_PIO4_CFGR_DRVSTR_LOW1	(0x1 << 16)
-#define		ATMEL_PIO4_CFGR_DRVSTR_MEDIUM	(0x2 << 16)
-#define		ATMEL_PIO4_CFGR_DRVSTR_HIGH	(0x3 << 16)
-#define ATMEL_PIO4_CFGR_EVTSEL	(0x7 << 24)
-#define		ATMEL_PIO4_CFGR_EVTSEL_FALLING	(0x0 << 24)
-#define		ATMEL_PIO4_CFGR_EVTSEL_RISING	(0x1 << 24)
-#define		ATMEL_PIO4_CFGR_EVTSEL_BOTH	(0x2 << 24)
-#define		ATMEL_PIO4_CFGR_EVTSEL_LOW	(0x3 << 24)
-#define		ATMEL_PIO4_CFGR_EVTSEL_HIGH	(0x4 << 24)
-#define ATMEL_PIO4_CFGR_PCFS	(0x1 << 29)
-#define ATMEL_PIO4_CFGR_ICFS	(0x1 << 30)
+DECLARE_GLOBAL_DATA_PTR;
 
 static struct atmel_pio4_port *atmel_pio4_port_base(u32 port)
 {
@@ -79,7 +50,7 @@ static int atmel_pio4_config_io_func(u32 port, u32 pin,
 	struct atmel_pio4_port *port_base;
 	u32 reg, mask;
 
-	if (pin >= ATMEL_PIO4_PINS_PER_BANK)
+	if (pin >= ATMEL_PIO_NPINS_PER_BANK)
 		return -ENODEV;
 
 	port_base = atmel_pio4_port_base(port);
@@ -88,7 +59,7 @@ static int atmel_pio4_config_io_func(u32 port, u32 pin,
 
 	mask = 1 << pin;
 	reg = func;
-	reg |= use_pullup ? ATMEL_PIO4_CFGR_PUEN : 0;
+	reg |= use_pullup ? ATMEL_PIO_PUEN_MASK : 0;
 
 	writel(mask, &port_base->mskr);
 	writel(reg, &port_base->cfgr);
@@ -99,56 +70,56 @@ static int atmel_pio4_config_io_func(u32 port, u32 pin,
 int atmel_pio4_set_gpio(u32 port, u32 pin, u32 use_pullup)
 {
 	return atmel_pio4_config_io_func(port, pin,
-					 ATMEL_PIO4_CFGR_FUNC_GPIO,
+					 ATMEL_PIO_CFGR_FUNC_GPIO,
 					 use_pullup);
 }
 
 int atmel_pio4_set_a_periph(u32 port, u32 pin, u32 use_pullup)
 {
 	return atmel_pio4_config_io_func(port, pin,
-					 ATMEL_PIO4_CFGR_FUNC_PERIPH_A,
+					 ATMEL_PIO_CFGR_FUNC_PERIPH_A,
 					 use_pullup);
 }
 
 int atmel_pio4_set_b_periph(u32 port, u32 pin, u32 use_pullup)
 {
 	return atmel_pio4_config_io_func(port, pin,
-					 ATMEL_PIO4_CFGR_FUNC_PERIPH_B,
+					 ATMEL_PIO_CFGR_FUNC_PERIPH_B,
 					 use_pullup);
 }
 
 int atmel_pio4_set_c_periph(u32 port, u32 pin, u32 use_pullup)
 {
 	return atmel_pio4_config_io_func(port, pin,
-					 ATMEL_PIO4_CFGR_FUNC_PERIPH_C,
+					 ATMEL_PIO_CFGR_FUNC_PERIPH_C,
 					 use_pullup);
 }
 
 int atmel_pio4_set_d_periph(u32 port, u32 pin, u32 use_pullup)
 {
 	return atmel_pio4_config_io_func(port, pin,
-					 ATMEL_PIO4_CFGR_FUNC_PERIPH_D,
+					 ATMEL_PIO_CFGR_FUNC_PERIPH_D,
 					 use_pullup);
 }
 
 int atmel_pio4_set_e_periph(u32 port, u32 pin, u32 use_pullup)
 {
 	return atmel_pio4_config_io_func(port, pin,
-					 ATMEL_PIO4_CFGR_FUNC_PERIPH_E,
+					 ATMEL_PIO_CFGR_FUNC_PERIPH_E,
 					 use_pullup);
 }
 
 int atmel_pio4_set_f_periph(u32 port, u32 pin, u32 use_pullup)
 {
 	return atmel_pio4_config_io_func(port, pin,
-					 ATMEL_PIO4_CFGR_FUNC_PERIPH_F,
+					 ATMEL_PIO_CFGR_FUNC_PERIPH_F,
 					 use_pullup);
 }
 
 int atmel_pio4_set_g_periph(u32 port, u32 pin, u32 use_pullup)
 {
 	return atmel_pio4_config_io_func(port, pin,
-					 ATMEL_PIO4_CFGR_FUNC_PERIPH_G,
+					 ATMEL_PIO_CFGR_FUNC_PERIPH_G,
 					 use_pullup);
 }
 
@@ -157,7 +128,7 @@ int atmel_pio4_set_pio_output(u32 port, u32 pin, u32 value)
 	struct atmel_pio4_port *port_base;
 	u32 reg, mask;
 
-	if (pin >= ATMEL_PIO4_PINS_PER_BANK)
+	if (pin >= ATMEL_PIO_NPINS_PER_BANK)
 		return -ENODEV;
 
 	port_base = atmel_pio4_port_base(port);
@@ -165,7 +136,7 @@ int atmel_pio4_set_pio_output(u32 port, u32 pin, u32 value)
 		return -ENODEV;
 
 	mask = 0x01 << pin;
-	reg = ATMEL_PIO4_CFGR_FUNC_GPIO | ATMEL_PIO4_CFGR_DIR;
+	reg = ATMEL_PIO_CFGR_FUNC_GPIO | ATMEL_PIO_DIR_MASK;
 
 	writel(mask, &port_base->mskr);
 	writel(reg, &port_base->cfgr);
@@ -183,7 +154,7 @@ int atmel_pio4_get_pio_input(u32 port, u32 pin)
 	struct atmel_pio4_port *port_base;
 	u32 reg, mask;
 
-	if (pin >= ATMEL_PIO4_PINS_PER_BANK)
+	if (pin >= ATMEL_PIO_NPINS_PER_BANK)
 		return -ENODEV;
 
 	port_base = atmel_pio4_port_base(port);
@@ -191,7 +162,7 @@ int atmel_pio4_get_pio_input(u32 port, u32 pin)
 		return -ENODEV;
 
 	mask = 0x01 << pin;
-	reg = ATMEL_PIO4_CFGR_FUNC_GPIO;
+	reg = ATMEL_PIO_CFGR_FUNC_GPIO;
 
 	writel(mask, &port_base->mskr);
 	writel(reg, &port_base->cfgr);
@@ -200,15 +171,37 @@ int atmel_pio4_get_pio_input(u32 port, u32 pin)
 }
 
 #ifdef CONFIG_DM_GPIO
+
+struct atmel_pioctrl_data {
+	u32 nbanks;
+};
+
+struct atmel_pio4_platdata {
+	struct atmel_pio4_port *reg_base;
+};
+
+static struct atmel_pio4_port *atmel_pio4_bank_base(struct udevice *dev,
+						    u32 bank)
+{
+	struct atmel_pio4_platdata *plat = dev_get_platdata(dev);
+	struct atmel_pio4_port *port_base =
+			(struct atmel_pio4_port *)((u32)plat->reg_base +
+			ATMEL_PIO_BANK_OFFSET * bank);
+
+	return port_base;
+}
+
 static int atmel_pio4_direction_input(struct udevice *dev, unsigned offset)
 {
-	struct at91_port_platdata *plat = dev_get_platdata(dev);
-	struct atmel_pio4_port *port_base = (atmel_pio4_port *)plat->base_addr;
-	u32 mask = 0x01 << offset;
-	u32 reg = ATMEL_PIO4_CFGR_FUNC_GPIO;
+	u32 bank = ATMEL_PIO_BANK(offset);
+	u32 line = ATMEL_PIO_LINE(offset);
+	struct atmel_pio4_port *port_base = atmel_pio4_bank_base(dev, bank);
+	u32 mask = BIT(line);
 
 	writel(mask, &port_base->mskr);
-	writel(reg, &port_base->cfgr);
+
+	clrbits_le32(&port_base->cfgr,
+		     ATMEL_PIO_CFGR_FUNC_MASK | ATMEL_PIO_DIR_MASK);
 
 	return 0;
 }
@@ -216,13 +209,15 @@ static int atmel_pio4_direction_input(struct udevice *dev, unsigned offset)
 static int atmel_pio4_direction_output(struct udevice *dev,
 				       unsigned offset, int value)
 {
-	struct at91_port_platdata *plat = dev_get_platdata(dev);
-	struct atmel_pio4_port *port_base = (atmel_pio4_port *)plat->base_addr;
-	u32 mask = 0x01 << offset;
-	u32 reg = ATMEL_PIO4_CFGR_FUNC_GPIO | ATMEL_PIO4_CFGR_DIR;
+	u32 bank = ATMEL_PIO_BANK(offset);
+	u32 line = ATMEL_PIO_LINE(offset);
+	struct atmel_pio4_port *port_base = atmel_pio4_bank_base(dev, bank);
+	u32 mask = BIT(line);
 
 	writel(mask, &port_base->mskr);
-	writel(reg, &port_base->cfgr);
+
+	clrsetbits_le32(&port_base->cfgr,
+			ATMEL_PIO_CFGR_FUNC_MASK, ATMEL_PIO_DIR_MASK);
 
 	if (value)
 		writel(mask, &port_base->sodr);
@@ -234,9 +229,10 @@ static int atmel_pio4_direction_output(struct udevice *dev,
 
 static int atmel_pio4_get_value(struct udevice *dev, unsigned offset)
 {
-	struct at91_port_platdata *plat = dev_get_platdata(dev);
-	struct atmel_pio4_port *port_base = (atmel_pio4_port *)plat->base_addr;
-	u32 mask = 0x01 << offset;
+	u32 bank = ATMEL_PIO_BANK(offset);
+	u32 line = ATMEL_PIO_LINE(offset);
+	struct atmel_pio4_port *port_base = atmel_pio4_bank_base(dev, bank);
+	u32 mask = BIT(line);
 
 	return (readl(&port_base->pdsr) & mask) ? 1 : 0;
 }
@@ -244,9 +240,10 @@ static int atmel_pio4_get_value(struct udevice *dev, unsigned offset)
 static int atmel_pio4_set_value(struct udevice *dev,
 				unsigned offset, int value)
 {
-	struct at91_port_platdata *plat = dev_get_platdata(dev);
-	struct atmel_pio4_port *port_base = (atmel_pio4_port *)plat->base_addr;
-	u32 mask = 0x01 << offset;
+	u32 bank = ATMEL_PIO_BANK(offset);
+	u32 line = ATMEL_PIO_LINE(offset);
+	struct atmel_pio4_port *port_base = atmel_pio4_bank_base(dev, bank);
+	u32 mask = BIT(line);
 
 	if (value)
 		writel(mask, &port_base->sodr);
@@ -258,14 +255,15 @@ static int atmel_pio4_set_value(struct udevice *dev,
 
 static int atmel_pio4_get_function(struct udevice *dev, unsigned offset)
 {
-	struct at91_port_platdata *plat = dev_get_platdata(dev);
-	struct atmel_pio4_port *port_base = (atmel_pio4_port *)plat->base_addr;
-	u32 mask = 0x01 << offset;
+	u32 bank = ATMEL_PIO_BANK(offset);
+	u32 line = ATMEL_PIO_LINE(offset);
+	struct atmel_pio4_port *port_base = atmel_pio4_bank_base(dev, bank);
+	u32 mask = BIT(line);
 
 	writel(mask, &port_base->mskr);
 
 	return (readl(&port_base->cfgr) &
-		ATMEL_PIO4_CFGR_DIR) ? GPIOF_OUTPUT : GPIOF_INPUT;
+		ATMEL_PIO_DIR_MASK) ? GPIOF_OUTPUT : GPIOF_INPUT;
 }
 
 static const struct dm_gpio_ops atmel_pio4_ops = {
@@ -276,21 +274,82 @@ static const struct dm_gpio_ops atmel_pio4_ops = {
 	.get_function		= atmel_pio4_get_function,
 };
 
+static int atmel_pio4_bind(struct udevice *dev)
+{
+	return dm_scan_fdt_node(dev, gd->fdt_blob, dev->of_offset, false);
+}
+
 static int atmel_pio4_probe(struct udevice *dev)
 {
-	struct at91_port_platdata *plat = dev_get_platdata(dev);
+	struct atmel_pio4_platdata *plat = dev_get_platdata(dev);
 	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
+	struct atmel_pioctrl_data *pioctrl_data;
+	struct udevice *dev_clk;
+	struct clk clk;
+	fdt_addr_t addr_base;
+	u32 nbanks;
+	int periph;
+	int ret;
+
+	ret = clk_get_by_index(dev, 0, &clk);
+	if (ret)
+		return ret;
+
+	periph = fdtdec_get_uint(gd->fdt_blob, clk.dev->of_offset, "reg", -1);
+	if (periph < 0)
+		return -EINVAL;
+
+	dev_clk = dev_get_parent(clk.dev);
+	ret = clk_request(dev_clk, &clk);
+	if (ret)
+		return ret;
+
+	clk.id = periph;
+	ret = clk_enable(&clk);
+	if (ret)
+		return ret;
+
+	clk_free(&clk);
+
+	addr_base = dev_get_addr(dev);
+	if (addr_base == FDT_ADDR_T_NONE)
+		return -EINVAL;
+
+	plat->reg_base = (struct atmel_pio4_port *)addr_base;
 
-	uc_priv->bank_name = plat->bank_name;
-	uc_priv->gpio_count = ATMEL_PIO4_PINS_PER_BANK;
+	pioctrl_data = (struct atmel_pioctrl_data *)dev_get_driver_data(dev);
+	nbanks = pioctrl_data->nbanks;
+
+	uc_priv->bank_name = fdt_get_name(gd->fdt_blob, dev->of_offset, NULL);
+	uc_priv->gpio_count = nbanks * ATMEL_PIO_NPINS_PER_BANK;
 
 	return 0;
 }
 
+/*
+ * The number of banks can be different from a SoC to another one.
+ * We can have up to 16 banks.
+ */
+static const struct atmel_pioctrl_data atmel_sama5d2_pioctrl_data = {
+	.nbanks	= 4,
+};
+
+static const struct udevice_id atmel_pio4_ids[] = {
+	{
+		.compatible = "atmel,sama5d2-gpio",
+		.data = (ulong)&atmel_sama5d2_pioctrl_data,
+	},
+	{}
+};
+
 U_BOOT_DRIVER(gpio_atmel_pio4) = {
 	.name	= "gpio_atmel_pio4",
 	.id	= UCLASS_GPIO,
 	.ops	= &atmel_pio4_ops,
 	.probe	= atmel_pio4_probe,
+	.bind	= atmel_pio4_bind,
+	.of_match = atmel_pio4_ids,
+	.platdata_auto_alloc_size = sizeof(struct atmel_pio4_platdata),
 };
+
 #endif
diff --git a/drivers/i2c/Kconfig b/drivers/i2c/Kconfig
index 67ebec7..1537b67 100644
--- a/drivers/i2c/Kconfig
+++ b/drivers/i2c/Kconfig
@@ -58,6 +58,16 @@ config DM_I2C_GPIO
 	  bindings are supported.
 	  Binding info: doc/device-tree-bindings/i2c/i2c-gpio.txt
 
+config SYS_I2C_AT91
+	bool "Atmel I2C driver"
+	depends on DM_I2C && ARCH_AT91
+	help
+	  Add support for the Atmel I2C driver. A serious problem is that there
+	  is no documented way to issue repeated START conditions for more than
+	  two messages, as needed to support combined I2C messages. Use the
+	  i2c-gpio driver unless your system can cope with this limitation.
+	  Binding info: doc/device-tree-bindings/i2c/i2c-at91.txt
+
 config SYS_I2C_FSL
        bool "Freescale I2C bus driver"
        depends on DM_I2C
diff --git a/drivers/i2c/Makefile b/drivers/i2c/Makefile
index 97b6bed..2987081 100644
--- a/drivers/i2c/Makefile
+++ b/drivers/i2c/Makefile
@@ -16,6 +16,7 @@ obj-$(CONFIG_PCA9564_I2C) += pca9564_i2c.o
 obj-$(CONFIG_TSI108_I2C) += tsi108_i2c.o
 obj-$(CONFIG_SH_SH7734_I2C) += sh_sh7734_i2c.o
 obj-$(CONFIG_SYS_I2C) += i2c_core.o
+obj-$(CONFIG_SYS_I2C_AT91) += at91_i2c.o
 obj-$(CONFIG_SYS_I2C_CADENCE) += i2c-cdns.o
 obj-$(CONFIG_SYS_I2C_DAVINCI) += davinci_i2c.o
 obj-$(CONFIG_SYS_I2C_DW) += designware_i2c.o
diff --git a/drivers/i2c/at91_i2c.c b/drivers/i2c/at91_i2c.c
new file mode 100644
index 0000000..8e9c3ad
--- /dev/null
+++ b/drivers/i2c/at91_i2c.c
@@ -0,0 +1,338 @@
+/*
+ * Atmel I2C driver.
+ *
+ * (C) Copyright 2016 Songjun Wu <songjun.wu@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <asm/io.h>
+#include <common.h>
+#include <clk_client.h>
+#include <dm.h>
+#include <errno.h>
+#include <fdtdec.h>
+#include <i2c.h>
+#include <linux/bitops.h>
+#include <mach/clk.h>
+
+#include "at91_i2c.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define I2C_TIMEOUT_MS	100
+
+static int at91_wait_for_xfer(struct at91_i2c_bus *bus, u32 status)
+{
+	struct at91_i2c_regs *reg = bus->regs;
+	ulong start_time = get_timer(0);
+	u32 sr;
+
+	bus->status = 0;
+
+	do {
+		sr = readl(&reg->sr);
+		bus->status |= sr;
+
+		if (sr & TWI_SR_NACK)
+			return -EREMOTEIO;
+		else if (sr & status)
+			return 0;
+	} while (get_timer(start_time) < I2C_TIMEOUT_MS);
+
+	return -ETIMEDOUT;
+}
+
+static int at91_i2c_xfer_msg(struct at91_i2c_bus *bus, struct i2c_msg *msg)
+{
+	struct at91_i2c_regs *reg = bus->regs;
+	bool is_read = msg->flags & I2C_M_RD;
+	u32 i;
+	int ret = 0;
+
+	readl(&reg->sr);
+	if (is_read) {
+		writel(TWI_CR_START, &reg->cr);
+
+		for (i = 0; !ret && i < (msg->len - 1); i++) {
+			ret = at91_wait_for_xfer(bus, TWI_SR_RXRDY);
+			msg->buf[i] = readl(&reg->rhr);
+		}
+
+		if (ret)
+			goto error;
+
+		writel(TWI_CR_STOP, &reg->cr);
+
+		ret = at91_wait_for_xfer(bus, TWI_SR_RXRDY);
+		if (ret)
+			goto error;
+
+		msg->buf[i] = readl(&reg->rhr);
+
+	} else {
+		writel(msg->buf[0], &reg->thr);
+		for (i = 1; !ret && (i < msg->len); i++) {
+			writel(msg->buf[i], &reg->thr);
+			ret = at91_wait_for_xfer(bus, TWI_SR_TXRDY);
+		}
+
+		if (ret)
+			goto error;
+
+		writel(TWI_CR_STOP, &reg->cr);
+	}
+
+	if (!ret)
+		ret = at91_wait_for_xfer(bus, TWI_SR_TXCOMP);
+
+	if (ret)
+		goto error;
+
+	if (bus->status & (TWI_SR_OVRE | TWI_SR_UNRE | TWI_SR_LOCK)) {
+		ret = -EIO;
+		goto error;
+	}
+
+	return 0;
+
+error:
+	if (bus->status & TWI_SR_LOCK)
+		writel(TWI_CR_LOCKCLR, &reg->cr);
+
+	return ret;
+}
+
+static int at91_i2c_xfer(struct udevice *dev, struct i2c_msg *msg, int nmsgs)
+{
+	struct at91_i2c_bus *bus = dev_get_priv(dev);
+	struct at91_i2c_regs *reg = bus->regs;
+	struct i2c_msg *m_start = msg;
+	bool is_read;
+	u32 int_addr_flag = 0;
+	int ret = 0;
+
+	if (nmsgs == 2) {
+		int internal_address = 0;
+		int i;
+
+		/* 1st msg is put into the internal address, start with 2nd */
+		m_start = &msg[1];
+
+		/* the max length of internal address is 3 bytes */
+		if (msg->len > 3)
+			return -EFAULT;
+
+		for (i = 0; i < msg->len; ++i) {
+			const unsigned addr = msg->buf[msg->len - 1 - i];
+
+			internal_address |= addr << (8 * i);
+			int_addr_flag += TWI_MMR_IADRSZ_1;
+		}
+
+		writel(internal_address, &reg->iadr);
+	}
+
+	is_read = m_start->flags & I2C_M_RD;
+
+	writel((m_start->addr << 16) | int_addr_flag |
+	       (is_read ? TWI_MMR_MREAD : 0), &reg->mmr);
+
+	ret = at91_i2c_xfer_msg(bus, m_start);
+
+	return ret;
+}
+
+/*
+ * Calculate symmetric clock as stated in datasheet:
+ * twi_clk = F_MAIN / (2 * (cdiv * (1 << ckdiv) + offset))
+ */
+static void at91_calc_i2c_clock(struct udevice *dev, int i2c_clk)
+{
+	struct at91_i2c_bus *bus = dev_get_priv(dev);
+	const struct at91_i2c_pdata *pdata = bus->pdata;
+	int offset = pdata->clk_offset;
+	int max_ckdiv = pdata->clk_max_div;
+	int ckdiv, cdiv, div;
+	unsigned long src_rate;
+
+	src_rate = bus->bus_clk_rate;
+
+	div = max(0, (int)DIV_ROUND_UP(src_rate, 2 * i2c_clk) - offset);
+	ckdiv = fls(div >> 8);
+	cdiv = div >> ckdiv;
+
+	if (ckdiv > max_ckdiv) {
+		ckdiv = max_ckdiv;
+		cdiv = 255;
+	}
+
+	bus->speed = DIV_ROUND_UP(src_rate,
+				  (cdiv * (1 << ckdiv) + offset) * 2);
+
+	bus->cwgr_val = (ckdiv << 16) | (cdiv << 8) | cdiv;
+}
+
+static int at91_i2c_enable_clk(struct udevice *dev)
+{
+	struct at91_i2c_bus *bus = dev_get_priv(dev);
+	struct udevice *dev_clk;
+	struct clk clk;
+	ulong clk_rate;
+	int periph;
+	int ret;
+
+	ret = clk_get_by_index(dev, 0, &clk);
+	if (ret)
+		return -EINVAL;
+
+	periph = fdtdec_get_uint(gd->fdt_blob, clk.dev->of_offset, "reg", -1);
+	if (periph < 0)
+		return -EINVAL;
+
+	dev_clk = dev_get_parent(clk.dev);
+	ret = clk_request(dev_clk, &clk);
+	if (ret)
+		return ret;
+
+	clk.id = periph;
+	ret = clk_enable(&clk);
+	if (ret)
+		return ret;
+
+	ret = clk_get_by_index(dev_clk, 0, &clk);
+	if (ret)
+		return ret;
+
+	clk_rate = clk_get_rate(&clk);
+	if (!clk_rate)
+		return -ENODEV;
+
+	bus->bus_clk_rate = clk_rate;
+
+	clk_free(&clk);
+
+	return 0;
+}
+
+static int at91_i2c_probe(struct udevice *dev, uint chip, uint chip_flags)
+{
+	struct at91_i2c_bus *bus = dev_get_priv(dev);
+	struct at91_i2c_regs *reg = bus->regs;
+	int ret;
+
+	ret = at91_i2c_enable_clk(dev);
+	if (ret)
+		return ret;
+
+	writel(TWI_CR_SWRST, &reg->cr);
+
+	at91_calc_i2c_clock(dev, bus->clock_frequency);
+
+	writel(bus->cwgr_val, &reg->cwgr);
+	writel(TWI_CR_MSEN, &reg->cr);
+	writel(TWI_CR_SVDIS, &reg->cr);
+
+	return 0;
+}
+
+static int at91_i2c_set_bus_speed(struct udevice *dev, unsigned int speed)
+{
+	struct at91_i2c_bus *bus = dev_get_priv(dev);
+
+	at91_calc_i2c_clock(dev, speed);
+
+	writel(bus->cwgr_val, &bus->regs->cwgr);
+
+	return 0;
+}
+
+int at91_i2c_get_bus_speed(struct udevice *dev)
+{
+	struct at91_i2c_bus *bus = dev_get_priv(dev);
+
+	return bus->speed;
+}
+
+static int at91_i2c_ofdata_to_platdata(struct udevice *dev)
+{
+	const void *blob = gd->fdt_blob;
+	struct at91_i2c_bus *bus = dev_get_priv(dev);
+	int node = dev->of_offset;
+
+	bus->regs = (struct at91_i2c_regs *)dev_get_addr(dev);
+	bus->pdata = (struct at91_i2c_pdata *)dev_get_driver_data(dev);
+	bus->clock_frequency = fdtdec_get_int(blob, node,
+					      "clock-frequency", 100000);
+
+	return 0;
+}
+
+static const struct dm_i2c_ops at91_i2c_ops = {
+	.xfer		= at91_i2c_xfer,
+	.probe_chip	= at91_i2c_probe,
+	.set_bus_speed	= at91_i2c_set_bus_speed,
+	.get_bus_speed	= at91_i2c_get_bus_speed,
+};
+
+static const struct at91_i2c_pdata at91rm9200_config = {
+	.clk_max_div = 5,
+	.clk_offset = 3,
+};
+
+static const struct at91_i2c_pdata at91sam9261_config = {
+	.clk_max_div = 5,
+	.clk_offset = 4,
+};
+
+static const struct at91_i2c_pdata at91sam9260_config = {
+	.clk_max_div = 7,
+	.clk_offset = 4,
+};
+
+static const struct at91_i2c_pdata at91sam9g20_config = {
+	.clk_max_div = 7,
+	.clk_offset = 4,
+};
+
+static const struct at91_i2c_pdata at91sam9g10_config = {
+	.clk_max_div = 7,
+	.clk_offset = 4,
+};
+
+static const struct at91_i2c_pdata at91sam9x5_config = {
+	.clk_max_div = 7,
+	.clk_offset = 4,
+};
+
+static const struct at91_i2c_pdata sama5d4_config = {
+	.clk_max_div = 7,
+	.clk_offset = 4,
+};
+
+static const struct at91_i2c_pdata sama5d2_config = {
+	.clk_max_div = 7,
+	.clk_offset = 3,
+};
+
+static const struct udevice_id at91_i2c_ids[] = {
+{ .compatible = "atmel,at91rm9200-i2c", .data = (long)&at91rm9200_config },
+{ .compatible = "atmel,at91sam9260-i2c", .data = (long)&at91sam9260_config },
+{ .compatible = "atmel,at91sam9261-i2c", .data = (long)&at91sam9261_config },
+{ .compatible = "atmel,at91sam9g20-i2c", .data = (long)&at91sam9g20_config },
+{ .compatible = "atmel,at91sam9g10-i2c", .data = (long)&at91sam9g10_config },
+{ .compatible = "atmel,at91sam9x5-i2c", .data = (long)&at91sam9x5_config },
+{ .compatible = "atmel,sama5d4-i2c", .data = (long)&sama5d4_config },
+{ .compatible = "atmel,sama5d2-i2c", .data = (long)&sama5d2_config },
+{ }
+};
+
+U_BOOT_DRIVER(i2c_at91) = {
+	.name	= "i2c_at91",
+	.id	= UCLASS_I2C,
+	.of_match = at91_i2c_ids,
+	.ofdata_to_platdata = at91_i2c_ofdata_to_platdata,
+	.per_child_auto_alloc_size = sizeof(struct dm_i2c_chip),
+	.priv_auto_alloc_size = sizeof(struct at91_i2c_bus),
+	.ops	= &at91_i2c_ops,
+};
diff --git a/drivers/i2c/at91_i2c.h b/drivers/i2c/at91_i2c.h
new file mode 100644
index 0000000..87f02bf
--- /dev/null
+++ b/drivers/i2c/at91_i2c.h
@@ -0,0 +1,77 @@
+#ifndef _AT91_I2C_H
+#define _AT91_I2C_H
+
+#define	TWI_CR_START		BIT(0)	/* Send a Start Condition */
+#define	TWI_CR_MSEN		BIT(2)	/* Master Transfer Enable */
+#define	TWI_CR_STOP		BIT(1)	/* Send a Stop Condition */
+#define	TWI_CR_SVDIS		BIT(5)	/* Slave Transfer Disable */
+#define	TWI_CR_SWRST		BIT(7)	/* Software Reset */
+#define	TWI_CR_ACMEN		BIT(16) /* Alternative Command Mode Enable */
+#define	TWI_CR_ACMDIS		BIT(17) /* Alternative Command Mode Disable */
+#define	TWI_CR_LOCKCLR		BIT(26) /* Lock Clear */
+
+#define	TWI_MMR_MREAD		BIT(12) /* Master Read Direction */
+#define	TWI_MMR_IADRSZ_1	BIT(8)	/* Internal Device Address Size */
+
+#define	TWI_SR_TXCOMP		BIT(0)	/* Transmission Complete */
+#define	TWI_SR_RXRDY		BIT(1)	/* Receive Holding Register Ready */
+#define	TWI_SR_TXRDY		BIT(2)	/* Transmit Holding Register Ready */
+#define	TWI_SR_OVRE		BIT(6)	/* Overrun Error */
+#define	TWI_SR_UNRE		BIT(7)	/* Underrun Error */
+#define	TWI_SR_NACK		BIT(8)	/* Not Acknowledged */
+#define	TWI_SR_LOCK		BIT(23) /* TWI Lock due to Frame Errors */
+
+#define	TWI_ACR_DATAL(len)	((len) & 0xff)
+#define	TWI_ACR_DIR_READ	BIT(8)
+
+#define	TWI_CWGR_HOLD_MAX	0x1f
+#define	TWI_CWGR_HOLD(x)	(((x) & TWI_CWGR_HOLD_MAX) << 24)
+
+struct at91_i2c_regs {
+	u32 cr;
+	u32 mmr;
+	u32 smr;
+	u32 iadr;
+	u32 cwgr;
+	u32 rev_0[3];
+	u32 sr;
+	u32 ier;
+	u32 idr;
+	u32 imr;
+	u32 rhr;
+	u32 thr;
+	u32 smbtr;
+	u32 rev_1;
+	u32 acr;
+	u32 filtr;
+	u32 rev_2;
+	u32 swmr;
+	u32 fmr;
+	u32 flr;
+	u32 rev_3;
+	u32 fsr;
+	u32 fier;
+	u32 fidr;
+	u32 fimr;
+	u32 rev_4[29];
+	u32 wpmr;
+	u32 wpsr;
+	u32 rev_5[6];
+};
+
+struct at91_i2c_pdata {
+	unsigned clk_max_div;
+	unsigned clk_offset;
+};
+
+struct at91_i2c_bus {
+	struct at91_i2c_regs *regs;
+	u32 status;
+	ulong bus_clk_rate;
+	u32 clock_frequency;
+	u32 speed;
+	u32 cwgr_val;
+	const struct at91_i2c_pdata *pdata;
+};
+
+#endif
diff --git a/drivers/mmc/Kconfig b/drivers/mmc/Kconfig
index dc8f2b6..3616dee 100644
--- a/drivers/mmc/Kconfig
+++ b/drivers/mmc/Kconfig
@@ -34,6 +34,16 @@ config MSM_SDHCI
           SD 3.0 specifications. Both SD and eMMC devices are supported.
 	  Card-detect gpios are not supported.
 
+config ATMEL_SDHCI
+	bool "Atmel SDHCI controller support"
+	depends on DM_MMC && BLK && DM_MMC_OPS && ARCH_AT91
+	help
+	  This enables support for the Atmel SDHCI controller, which supports
+	  the embedded MultiMedia Card (e.MMC) Specification V4.51, the SD
+	  Memory Card Specification V3.0, and the SDIO V3.0 specification.
+	  It is compliant with the SD Host Controller Standard V3.0
+	  specification.
+
 config ROCKCHIP_DWMMC
 	bool "Rockchip SD/MMC controller support"
 	depends on DM_MMC && OF_CONTROL
diff --git a/drivers/mmc/atmel_sdhci.c b/drivers/mmc/atmel_sdhci.c
index 24b68b6..dd6bd33 100644
--- a/drivers/mmc/atmel_sdhci.c
+++ b/drivers/mmc/atmel_sdhci.c
@@ -6,12 +6,15 @@
  */
 
 #include <common.h>
+#include <clk.h>
+#include <dm.h>
 #include <malloc.h>
 #include <sdhci.h>
 #include <asm/arch/clk.h>
 
 #define ATMEL_SDHC_MIN_FREQ	400000
 
+#ifndef CONFIG_DM_MMC
 int atmel_sdhci_init(void *regbase, u32 id)
 {
 	struct sdhci_host *host;
@@ -38,3 +41,123 @@ int atmel_sdhci_init(void *regbase, u32 id)
 
 	return 0;
 }
+
+#else
+
+DECLARE_GLOBAL_DATA_PTR;
+
+struct atmel_sdhci_plat {
+	struct mmc_config cfg;
+	struct mmc mmc;
+};
+
+static int atmel_sdhci_get_clk(struct udevice *dev, int index, struct clk *clk)
+{
+	struct udevice *dev_clk;
+	int periph, ret;
+
+	ret = clk_get_by_index(dev, index, clk);
+	if (ret)
+		return ret;
+
+	periph = fdtdec_get_uint(gd->fdt_blob, clk->dev->of_offset, "reg", -1);
+	if (periph < 0)
+		return -EINVAL;
+
+	dev_clk = dev_get_parent(clk->dev);
+	ret = clk_request(dev_clk, clk);
+	if (ret)
+		return ret;
+
+	clk->id = periph;
+
+	return 0;
+}
+
+static int atmel_sdhci_probe(struct udevice *dev)
+{
+	struct mmc_uclass_priv *upriv = dev_get_uclass_priv(dev);
+	struct atmel_sdhci_plat *plat = dev_get_platdata(dev);
+	struct sdhci_host *host = dev_get_priv(dev);
+	u32 max_clk;
+	u32 caps, caps_1;
+	u32 clk_base, clk_mul;
+	ulong gck_rate;
+	struct clk clk;
+	int ret;
+
+	ret = atmel_sdhci_get_clk(dev, 0, &clk);
+	if (ret)
+		return ret;
+
+	ret = clk_enable(&clk);
+	if (ret)
+		return ret;
+
+	host->name = dev->name;
+	host->ioaddr = (void *)dev_get_addr(dev);
+
+	host->quirks = 0;
+	host->bus_width	= fdtdec_get_int(gd->fdt_blob, dev->of_offset,
+					 "bus-width", 4);
+
+	caps = sdhci_readl(host, SDHCI_CAPABILITIES);
+	clk_base = (caps & SDHCI_CLOCK_V3_BASE_MASK) >> SDHCI_CLOCK_BASE_SHIFT;
+	caps_1 = sdhci_readl(host, SDHCI_CAPABILITIES_1);
+	clk_mul = (caps_1 & SDHCI_CLOCK_MUL_MASK) >> SDHCI_CLOCK_MUL_SHIFT;
+	gck_rate = clk_base * 1000000 * (clk_mul + 1);
+
+	ret = atmel_sdhci_get_clk(dev, 1, &clk);
+	if (ret)
+		return ret;
+
+	ret = clk_set_rate(&clk, gck_rate);
+	if (ret)
+		return ret;
+
+	max_clk = clk_get_rate(&clk);
+	if (!max_clk)
+		return -EINVAL;
+
+	ret = sdhci_setup_cfg(&plat->cfg, host, max_clk, ATMEL_SDHC_MIN_FREQ);
+	if (ret)
+		return ret;
+
+	host->mmc = &plat->mmc;
+	host->mmc->dev = dev;
+	host->mmc->priv = host;
+	upriv->mmc = host->mmc;
+
+	clk_free(&clk);
+
+	return sdhci_probe(dev);
+}
+
+static int atmel_sdhci_bind(struct udevice *dev)
+{
+	struct atmel_sdhci_plat *plat = dev_get_platdata(dev);
+	int ret;
+
+	ret = sdhci_bind(dev, &plat->mmc, &plat->cfg);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static const struct udevice_id atmel_sdhci_ids[] = {
+	{ .compatible = "atmel,sama5d2-sdhci" },
+	{ }
+};
+
+U_BOOT_DRIVER(atmel_sdhci_drv) = {
+	.name		= "atmel_sdhci",
+	.id		= UCLASS_MMC,
+	.of_match	= atmel_sdhci_ids,
+	.ops		= &sdhci_ops,
+	.bind		= atmel_sdhci_bind,
+	.probe		= atmel_sdhci_probe,
+	.priv_auto_alloc_size = sizeof(struct sdhci_host),
+	.platdata_auto_alloc_size = sizeof(struct atmel_sdhci_plat),
+};
+#endif
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index 2972dba..44b53b9 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -141,6 +141,13 @@ config ROCKCHIP_RK3288_PINCTRL
 	  definitions and pin control functions for each available multiplex
 	  function.
 
+config PINCTRL_AT91PIO4
+	bool "AT91 PIO4 pinctrl driver"
+	depends on DM
+	help
+	  This option is to enable the AT91 pinctrl driver for AT91 PIO4
+	  controller which is available on SAMA5D2 SoC.
+
 config PINCTRL_SANDBOX
 	bool "Sandbox pinctrl driver"
 	depends on SANDBOX
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index 7f94681..c92ab8c 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -5,6 +5,7 @@
 obj-y					+= pinctrl-uclass.o
 obj-$(CONFIG_$(SPL_)PINCTRL_GENERIC)	+= pinctrl-generic.o
 
+obj-$(CONFIG_PINCTRL_AT91PIO4)		+= pinctrl-at91-pio4.o
 obj-y					+= nxp/
 obj-$(CONFIG_ARCH_ATH79) += ath79/
 obj-$(CONFIG_ARCH_ROCKCHIP) += rockchip/
diff --git a/drivers/pinctrl/pinctrl-at91-pio4.c b/drivers/pinctrl/pinctrl-at91-pio4.c
new file mode 100644
index 0000000..6d4aadc
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-at91-pio4.c
@@ -0,0 +1,182 @@
+/*
+ * Atmel PIO4 pinctrl driver
+ *
+ * Copyright (C) 2016 Atmel Corporation
+ *               Wenyou.Yang <wenyou.yang@atmel.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm/device.h>
+#include <dm/pinctrl.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <mach/atmel_pio4.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+/*
+ * Warning:
+ * In order to not introduce confusion between Atmel PIO groups and pinctrl
+ * framework groups, Atmel PIO groups will be called banks.
+ */
+
+struct atmel_pio4_platdata {
+	struct atmel_pio4_port *reg_base;
+};
+
+static const struct pinconf_param conf_params[] = {
+	{ "bias-disable", PIN_CONFIG_BIAS_DISABLE, 0 },
+	{ "bias-pull-up", PIN_CONFIG_BIAS_PULL_UP, 1 },
+	{ "bias-pull-down", PIN_CONFIG_BIAS_PULL_DOWN, 1 },
+	{ "drive-open-drain", PIN_CONFIG_DRIVE_OPEN_DRAIN, 0 },
+	{ "input-schmitt-disable", PIN_CONFIG_INPUT_SCHMITT_ENABLE, 0 },
+	{ "input-schmitt-enable", PIN_CONFIG_INPUT_SCHMITT_ENABLE, 1 },
+	{ "input-debounce", PIN_CONFIG_INPUT_DEBOUNCE, 0 },
+};
+
+static u32 atmel_pinctrl_get_pinconf(const void *blob, int node)
+{
+	const struct pinconf_param *params;
+	u32 param, arg, conf = 0;
+	u32 i;
+
+	for (i = 0; i < ARRAY_SIZE(conf_params); i++) {
+		params = &conf_params[i];
+		if (!fdt_get_property(blob, node, params->property, NULL))
+			continue;
+
+		param = params->param;
+		arg = params->default_value;
+
+		switch (param) {
+		case PIN_CONFIG_BIAS_DISABLE:
+			conf &= (~ATMEL_PIO_PUEN_MASK);
+			conf &= (~ATMEL_PIO_PDEN_MASK);
+			break;
+		case PIN_CONFIG_BIAS_PULL_UP:
+			conf |= ATMEL_PIO_PUEN_MASK;
+			break;
+		case PIN_CONFIG_BIAS_PULL_DOWN:
+			conf |= ATMEL_PIO_PDEN_MASK;
+			break;
+		case PIN_CONFIG_DRIVE_OPEN_DRAIN:
+			if (arg == 0)
+				conf &= (~ATMEL_PIO_OPD_MASK);
+			else
+				conf |= ATMEL_PIO_OPD_MASK;
+			break;
+		case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+			if (arg == 0)
+				conf |= ATMEL_PIO_SCHMITT_MASK;
+			else
+				conf &= (~ATMEL_PIO_SCHMITT_MASK);
+			break;
+		case PIN_CONFIG_INPUT_DEBOUNCE:
+			if (arg == 0) {
+				conf &= (~ATMEL_PIO_IFEN_MASK);
+				conf &= (~ATMEL_PIO_IFSCEN_MASK);
+			} else {
+				conf |= ATMEL_PIO_IFEN_MASK;
+				conf |= ATMEL_PIO_IFSCEN_MASK;
+			}
+			break;
+		default:
+			printf("%s: Unsupported configuration parameter: %u\n",
+			       __func__, param);
+			break;
+		}
+	}
+
+	return conf;
+}
+
+static inline struct atmel_pio4_port *atmel_pio4_bank_base(struct udevice *dev,
+							   u32 bank)
+{
+	struct atmel_pio4_platdata *plat = dev_get_platdata(dev);
+	struct atmel_pio4_port *bank_base =
+			(struct atmel_pio4_port *)((u32)plat->reg_base +
+			ATMEL_PIO_BANK_OFFSET * bank);
+
+	return bank_base;
+}
+
+#define MAX_PINMUX_ENTRIES	40
+
+static int atmel_pinctrl_set_state(struct udevice *dev, struct udevice *config)
+{
+	struct atmel_pio4_port *bank_base;
+	const void *blob = gd->fdt_blob;
+	int node = config->of_offset;
+	u32 offset, func, bank, line;
+	u32 cells[MAX_PINMUX_ENTRIES];
+	u32 i, conf;
+	int count;
+
+	conf = atmel_pinctrl_get_pinconf(blob, node);
+
+	count = fdtdec_get_int_array_count(blob, node, "pinmux",
+					   cells, ARRAY_SIZE(cells));
+	if (count < 0) {
+		printf("%s: bad pinmux array %d\n", __func__, count);
+		return -EINVAL;
+	}
+
+	if (count > MAX_PINMUX_ENTRIES) {
+		printf("%s: unsupported pinmux array count %d\n",
+		       __func__, count);
+		return -EINVAL;
+	}
+
+	for (i = 0 ; i < count; i++) {
+		offset = ATMEL_GET_PIN_NO(cells[i]);
+		func = ATMEL_GET_PIN_FUNC(cells[i]);
+
+		bank = ATMEL_PIO_BANK(offset);
+		line = ATMEL_PIO_LINE(offset);
+
+		bank_base = atmel_pio4_bank_base(dev, bank);
+
+		writel(BIT(line), &bank_base->mskr);
+		conf &= (~ATMEL_PIO_CFGR_FUNC_MASK);
+		conf |= (func & ATMEL_PIO_CFGR_FUNC_MASK);
+		writel(conf, &bank_base->cfgr);
+	}
+
+	return 0;
+}
+
+const struct pinctrl_ops atmel_pinctrl_ops  = {
+	.set_state = atmel_pinctrl_set_state,
+};
+
+static int atmel_pinctrl_probe(struct udevice *dev)
+{
+	struct atmel_pio4_platdata *plat = dev_get_platdata(dev);
+	fdt_addr_t addr_base;
+
+	dev = dev_get_parent(dev);
+	addr_base = dev_get_addr(dev);
+	if (addr_base == FDT_ADDR_T_NONE)
+		return -EINVAL;
+
+	plat->reg_base = (struct atmel_pio4_port *)addr_base;
+
+	return 0;
+}
+
+static const struct udevice_id atmel_pinctrl_match[] = {
+	{ .compatible = "atmel,sama5d2-pinctrl" },
+	{}
+};
+
+U_BOOT_DRIVER(atmel_pinctrl) = {
+	.name = "pinctrl_atmel_pio4",
+	.id = UCLASS_PINCTRL,
+	.of_match = atmel_pinctrl_match,
+	.probe = atmel_pinctrl_probe,
+	.platdata_auto_alloc_size = sizeof(struct atmel_pio4_platdata),
+	.ops = &atmel_pinctrl_ops,
+};
diff --git a/drivers/tpm/Kconfig b/drivers/tpm/Kconfig
index 9a7b7f5..7ab34ce 100644
--- a/drivers/tpm/Kconfig
+++ b/drivers/tpm/Kconfig
@@ -15,7 +15,7 @@ config TPM_TIS_SANDBOX
 
 config TPM_ATMEL_TWI
 	bool "Enable Atmel TWI TPM device driver"
-	depends on TPM && DM_I2C
+	depends on TPM
 	help
 	  This driver supports an Atmel TPM device connected on the I2C bus.
 	  The usual tpm operations and the 'tpm' command can be used to talk
diff --git a/drivers/tpm/tpm_atmel_twi.c b/drivers/tpm/tpm_atmel_twi.c
index 2aa9381..eba654b 100644
--- a/drivers/tpm/tpm_atmel_twi.c
+++ b/drivers/tpm/tpm_atmel_twi.c
@@ -81,14 +81,23 @@ static int tpm_atmel_twi_xfer(struct udevice *dev,
 	print_buffer(0, (void *)sendbuf, 1, send_size, 0);
 #endif
 
+#ifndef CONFIG_DM_I2C
 	res = i2c_write(0x29, 0, 0, (uchar *)sendbuf, send_size);
+#else
+	res = dm_i2c_write(dev, 0, sendbuf, send_size);
+#endif
 	if (res) {
 		printf("i2c_write returned %d\n", res);
 		return -1;
 	}
 
 	start = get_timer(0);
-	while ((res = i2c_read(0x29, 0, 0, recvbuf, 10))) {
+#ifndef CONFIG_DM_I2C
+	while ((res = i2c_read(0x29, 0, 0, recvbuf, 10)))
+#else
+	while ((res = dm_i2c_read(dev, 0, recvbuf, 10)))
+#endif
+	{
 		/* TODO Use TIS_TIMEOUT from tpm_tis_infineon.h */
 		if (get_timer(start) > ATMEL_TPM_TIMEOUT_MS) {
 			puts("tpm timed out\n");
@@ -99,7 +108,11 @@ static int tpm_atmel_twi_xfer(struct udevice *dev,
 	if (!res) {
 		*recv_len = get_unaligned_be32(recvbuf + 2);
 		if (*recv_len > 10)
+#ifndef CONFIG_DM_I2C
 			res = i2c_read(0x29, 0, 0, recvbuf, *recv_len);
+#else
+			res = dm_i2c_read(dev, 0, recvbuf, *recv_len);
+#endif
 	}
 	if (res) {
 		printf("i2c_read returned %d (rlen=%d)\n", res, *recv_len);
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index 31eec0a..e0699d4 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -57,6 +57,13 @@ config USB_EHCI
 
 if USB_EHCI_HCD
 
+config USB_EHCI_ATMEL
+	bool  "Support for Atmel on-chip EHCI USB controller"
+	depends on ARCH_AT91
+	default y
+	---help---
+	  Enables support for the on-chip EHCI controller on Atmel chips.
+
 config USB_EHCI_MARVELL
 	bool "Support for MVEBU (AXP / A38x) on-chip EHCI USB controller"
 	depends on ARCH_MVEBU
diff --git a/drivers/usb/host/ehci-atmel.c b/drivers/usb/host/ehci-atmel.c
index 1d7d280..d65bbe9 100644
--- a/drivers/usb/host/ehci-atmel.c
+++ b/drivers/usb/host/ehci-atmel.c
@@ -7,12 +7,18 @@
  */
 
 #include <common.h>
+#include <clk.h>
+#include <dm.h>
 #include <usb.h>
 #include <asm/io.h>
 #include <asm/arch/clk.h>
 
 #include "ehci.h"
 
+DECLARE_GLOBAL_DATA_PTR;
+
+#ifndef CONFIG_DM_USB
+
 int ehci_hcd_init(int index, enum usb_init_type init,
 		struct ehci_hccr **hccr, struct ehci_hcor **hcor)
 {
@@ -41,3 +47,113 @@ int ehci_hcd_stop(int index)
 
 	return 0;
 }
+
+#else
+
+struct ehci_atmel_priv {
+	struct ehci_ctrl ehci;
+};
+
+static int ehci_atmel_enable_clk(struct udevice *dev)
+{
+	struct udevice *dev_clk;
+	struct clk clk;
+	int periph;
+	int ret;
+
+	ret = clk_get_by_index(dev, 0, &clk);
+	if (ret)
+		return ret;
+
+	ret = clk_enable(&clk);
+	if (ret)
+		return ret;
+
+	ret = clk_get_by_index(dev, 1, &clk);
+	if (ret)
+		return -EINVAL;
+
+	periph = fdtdec_get_uint(gd->fdt_blob, clk.dev->of_offset, "reg", -1);
+	if (periph < 0)
+		return -EINVAL;
+
+	dev_clk = dev_get_parent(clk.dev);
+	if (!dev_clk)
+		return -ENODEV;
+
+	ret = clk_request(dev_clk, &clk);
+	if (ret)
+		return ret;
+
+	clk.id = periph;
+	ret = clk_enable(&clk);
+	if (ret)
+		return ret;
+
+	clk_free(&clk);
+
+	return 0;
+}
+
+static int ehci_atmel_probe(struct udevice *dev)
+{
+	struct ehci_hccr *hccr;
+	struct ehci_hcor *hcor;
+	fdt_addr_t hcd_base;
+	int ret;
+
+	ret = ehci_atmel_enable_clk(dev);
+	if (ret) {
+		debug("Failed to enable USB Host clock\n");
+		return ret;
+	}
+
+	/*
+	 * Get the base address for EHCI controller from the device node
+	 */
+	hcd_base = dev_get_addr(dev);
+	if (hcd_base == FDT_ADDR_T_NONE) {
+		debug("Can't get the EHCI register base address\n");
+		return -ENXIO;
+	}
+
+	hccr = (struct ehci_hccr *)hcd_base;
+	hcor = (struct ehci_hcor *)
+		((u32)hccr + HC_LENGTH(ehci_readl(&hccr->cr_capbase)));
+
+	debug("echi-atmel: init hccr %x and hcor %x hc_length %d\n",
+	      (u32)hccr, (u32)hcor,
+	      (u32)HC_LENGTH(ehci_readl(&hccr->cr_capbase)));
+
+	return ehci_register(dev, hccr, hcor, NULL, 0, USB_INIT_HOST);
+}
+
+static int ehci_atmel_remove(struct udevice *dev)
+{
+	int ret;
+
+	ret = ehci_deregister(dev);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static const struct udevice_id ehci_usb_ids[] = {
+	{ .compatible = "atmel,at91sam9g45-ehci", },
+	{ }
+};
+
+U_BOOT_DRIVER(ehci_atmel) = {
+	.name		= "ehci_atmel",
+	.id		= UCLASS_USB,
+	.of_match	= ehci_usb_ids,
+	.probe		= ehci_atmel_probe,
+	.remove		= ehci_atmel_remove,
+	.ops		= &ehci_usb_ops,
+	.platdata_auto_alloc_size = sizeof(struct usb_platdata),
+	.priv_auto_alloc_size = sizeof(struct ehci_atmel_priv),
+	.flags		= DM_FLAG_ALLOC_PRIV_DMA,
+};
+
+#endif