5 files changed, 435 insertions, 117 deletions

diff --git a/drivers/gpio/at91_gpio.c b/drivers/gpio/at91_gpio.c
index 75a32ee815..8e52e3dad0 100644
--- a/drivers/gpio/at91_gpio.c
+++ b/drivers/gpio/at91_gpio.c
@@ -59,6 +59,11 @@ int at91_set_pio_pullup(unsigned port, unsigned pin, int use_pullup)
 {
 	struct at91_port *at91_port = at91_pio_get_port(port);
 
+#if defined(CPU_HAS_PIO3)
+	if (use_pullup)
+		at91_set_pio_pulldown(port, pin, 0);
+#endif
+
 	if (at91_port && (pin < GPIO_PER_BANK))
 		at91_set_port_pullup(at91_port, pin, use_pullup);
 
@@ -305,10 +310,10 @@ int at91_set_pio_pulldown(unsigned port, unsigned pin, int is_on)
 
 	if (at91_port && (pin < GPIO_PER_BANK)) {
 		mask = 1 << pin;
-		writel(mask, &at91_port->pudr);
-		if (is_on)
+		if (is_on) {
+			at91_set_pio_pullup(port, pin, 0);
 			writel(mask, &at91_port->ppder);
-		else
+		} else
 			writel(mask, &at91_port->ppddr);
 	}
 
diff --git a/drivers/mtd/nand/atmel_nand.c b/drivers/mtd/nand/atmel_nand.c
index 75e830724c..ad5ded3a56 100644
--- a/drivers/mtd/nand/atmel_nand.c
+++ b/drivers/mtd/nand/atmel_nand.c
@@ -24,9 +24,9 @@
 
 /* Register access macros */
 #define ecc_readl(add, reg)				\
-	readl(AT91_BASE_SYS + add + ATMEL_ECC_##reg)
+	readl(add + ATMEL_ECC_##reg)
 #define ecc_writel(add, reg, value)			\
-	writel((value), AT91_BASE_SYS + add + ATMEL_ECC_##reg)
+	writel((value), add + ATMEL_ECC_##reg)
 
 #include "atmel_nand_ecc.h"	/* Hardware ECC registers */
 
@@ -1156,6 +1156,7 @@ int atmel_hwecc_nand_init_param(struct nand_chip *nand, struct mtd_info *mtd)
 	nand->ecc.hwctl = atmel_nand_hwctl;
 	nand->ecc.read_page = atmel_nand_read_page;
 	nand->ecc.bytes = 4;
+	nand->ecc.strength = 4;
 
 	if (nand->ecc.mode == NAND_ECC_HW) {
 		/* ECC is calculated for the whole page (1 step) */
diff --git a/drivers/net/macb.c b/drivers/net/macb.c
index 4bf8fa45d7..0835fdc306 100644
--- a/drivers/net/macb.c
+++ b/drivers/net/macb.c
@@ -4,6 +4,7 @@
  * SPDX-License-Identifier:	GPL-2.0+
  */
 #include <common.h>
+#include <dm.h>
 
 /*
  * The u-boot networking stack is a little weird.  It seems like the
@@ -28,7 +29,9 @@
  */
 
 #include <net.h>
+#ifndef CONFIG_DM_ETH
 #include <netdev.h>
+#endif
 #include <malloc.h>
 #include <miiphy.h>
 
@@ -84,6 +87,8 @@ struct macb_device {
 	unsigned int		rx_tail;
 	unsigned int		tx_head;
 	unsigned int		tx_tail;
+	unsigned int		next_rx_tail;
+	bool			wrapped;
 
 	void			*rx_buffer;
 	void			*tx_buffer;
@@ -98,11 +103,15 @@ struct macb_device {
 	unsigned long		dummy_desc_dma;
 
 	const struct device	*dev;
+#ifndef CONFIG_DM_ETH
 	struct eth_device	netdev;
+#endif
 	unsigned short		phy_addr;
 	struct mii_dev		*bus;
 };
+#ifndef CONFIG_DM_ETH
 #define to_macb(_nd) container_of(_nd, struct macb_device, netdev)
+#endif
 
 static int macb_is_gem(struct macb_device *macb)
 {
@@ -192,8 +201,13 @@ void __weak arch_get_mdio_control(const char *name)
 
 int macb_miiphy_read(const char *devname, u8 phy_adr, u8 reg, u16 *value)
 {
+#ifdef CONFIG_DM_ETH
+	struct udevice *dev = eth_get_dev_by_name(devname);
+	struct macb_device *macb = dev_get_priv(dev);
+#else
 	struct eth_device *dev = eth_get_dev_by_name(devname);
 	struct macb_device *macb = to_macb(dev);
+#endif
 
 	if (macb->phy_addr != phy_adr)
 		return -1;
@@ -206,8 +220,13 @@ int macb_miiphy_read(const char *devname, u8 phy_adr, u8 reg, u16 *value)
 
 int macb_miiphy_write(const char *devname, u8 phy_adr, u8 reg, u16 value)
 {
+#ifdef CONFIG_DM_ETH
+	struct udevice *dev = eth_get_dev_by_name(devname);
+	struct macb_device *macb = dev_get_priv(dev);
+#else
 	struct eth_device *dev = eth_get_dev_by_name(devname);
 	struct macb_device *macb = to_macb(dev);
+#endif
 
 	if (macb->phy_addr != phy_adr)
 		return -1;
@@ -255,9 +274,9 @@ static inline void macb_invalidate_rx_buffer(struct macb_device *macb)
 
 #if defined(CONFIG_CMD_NET)
 
-static int macb_send(struct eth_device *netdev, void *packet, int length)
+static int _macb_send(struct macb_device *macb, const char *name, void *packet,
+		      int length)
 {
-	struct macb_device *macb = to_macb(netdev);
 	unsigned long paddr, ctrl;
 	unsigned int tx_head = macb->tx_head;
 	int i;
@@ -278,7 +297,7 @@ static int macb_send(struct eth_device *netdev, void *packet, int length)
 	barrier();
 	macb_flush_ring_desc(macb, TX);
 	/* Do we need check paddr and length is dcache line aligned? */
-	flush_dcache_range(paddr, paddr + length);
+	flush_dcache_range(paddr, paddr + ALIGN(length, ARCH_DMA_MINALIGN));
 	macb_writel(macb, NCR, MACB_BIT(TE) | MACB_BIT(RE) | MACB_BIT(TSTART));
 
 	/*
@@ -298,12 +317,11 @@ static int macb_send(struct eth_device *netdev, void *packet, int length)
 
 	if (i <= MACB_TX_TIMEOUT) {
 		if (ctrl & TXBUF_UNDERRUN)
-			printf("%s: TX underrun\n", netdev->name);
+			printf("%s: TX underrun\n", name);
 		if (ctrl & TXBUF_EXHAUSTED)
-			printf("%s: TX buffers exhausted in mid frame\n",
-			       netdev->name);
+			printf("%s: TX buffers exhausted in mid frame\n", name);
 	} else {
-		printf("%s: TX timeout\n", netdev->name);
+		printf("%s: TX timeout\n", name);
 	}
 
 	/* No one cares anyway */
@@ -335,26 +353,25 @@ static void reclaim_rx_buffers(struct macb_device *macb,
 	macb->rx_tail = new_tail;
 }
 
-static int macb_recv(struct eth_device *netdev)
+static int _macb_recv(struct macb_device *macb, uchar **packetp)
 {
-	struct macb_device *macb = to_macb(netdev);
-	unsigned int rx_tail = macb->rx_tail;
+	unsigned int next_rx_tail = macb->next_rx_tail;
 	void *buffer;
 	int length;
-	int wrapped = 0;
 	u32 status;
 
+	macb->wrapped = false;
 	for (;;) {
 		macb_invalidate_ring_desc(macb, RX);
 
-		if (!(macb->rx_ring[rx_tail].addr & RXADDR_USED))
-			return -1;
+		if (!(macb->rx_ring[next_rx_tail].addr & RXADDR_USED))
+			return -EAGAIN;
 
-		status = macb->rx_ring[rx_tail].ctrl;
+		status = macb->rx_ring[next_rx_tail].ctrl;
 		if (status & RXBUF_FRAME_START) {
-			if (rx_tail != macb->rx_tail)
-				reclaim_rx_buffers(macb, rx_tail);
-			wrapped = 0;
+			if (next_rx_tail != macb->rx_tail)
+				reclaim_rx_buffers(macb, next_rx_tail);
+			macb->wrapped = false;
 		}
 
 		if (status & RXBUF_FRAME_END) {
@@ -362,7 +379,7 @@ static int macb_recv(struct eth_device *netdev)
 			length = status & RXBUF_FRMLEN_MASK;
 
 			macb_invalidate_rx_buffer(macb);
-			if (wrapped) {
+			if (macb->wrapped) {
 				unsigned int headlen, taillen;
 
 				headlen = 128 * (MACB_RX_RING_SIZE
@@ -372,34 +389,33 @@ static int macb_recv(struct eth_device *netdev)
 				       buffer, headlen);
 				memcpy((void *)net_rx_packets[0] + headlen,
 				       macb->rx_buffer, taillen);
-				buffer = (void *)net_rx_packets[0];
+				*packetp = (void *)net_rx_packets[0];
+			} else {
+				*packetp = buffer;
 			}
 
-			net_process_received_packet(buffer, length);
-			if (++rx_tail >= MACB_RX_RING_SIZE)
-				rx_tail = 0;
-			reclaim_rx_buffers(macb, rx_tail);
+			if (++next_rx_tail >= MACB_RX_RING_SIZE)
+				next_rx_tail = 0;
+			macb->next_rx_tail = next_rx_tail;
+			return length;
 		} else {
-			if (++rx_tail >= MACB_RX_RING_SIZE) {
-				wrapped = 1;
-				rx_tail = 0;
+			if (++next_rx_tail >= MACB_RX_RING_SIZE) {
+				macb->wrapped = true;
+				next_rx_tail = 0;
 			}
 		}
 		barrier();
 	}
-
-	return 0;
 }
 
-static void macb_phy_reset(struct macb_device *macb)
+static void macb_phy_reset(struct macb_device *macb, const char *name)
 {
-	struct eth_device *netdev = &macb->netdev;
 	int i;
 	u16 status, adv;
 
 	adv = ADVERTISE_CSMA | ADVERTISE_ALL;
 	macb_mdio_write(macb, MII_ADVERTISE, adv);
-	printf("%s: Starting autonegotiation...\n", netdev->name);
+	printf("%s: Starting autonegotiation...\n", name);
 	macb_mdio_write(macb, MII_BMCR, (BMCR_ANENABLE
 					 | BMCR_ANRESTART));
 
@@ -411,10 +427,10 @@ static void macb_phy_reset(struct macb_device *macb)
 	}
 
 	if (status & BMSR_ANEGCOMPLETE)
-		printf("%s: Autonegotiation complete\n", netdev->name);
+		printf("%s: Autonegotiation complete\n", name);
 	else
 		printf("%s: Autonegotiation timed out (status=0x%04x)\n",
-		       netdev->name, status);
+		       name, status);
 }
 
 #ifdef CONFIG_MACB_SEARCH_PHY
@@ -441,9 +457,8 @@ static int macb_phy_find(struct macb_device *macb)
 #endif /* CONFIG_MACB_SEARCH_PHY */
 
 
-static int macb_phy_init(struct macb_device *macb)
+static int macb_phy_init(struct macb_device *macb, const char *name)
 {
-	struct eth_device *netdev = &macb->netdev;
 #ifdef CONFIG_PHYLIB
 	struct phy_device *phydev;
 #endif
@@ -452,7 +467,7 @@ static int macb_phy_init(struct macb_device *macb)
 	int media, speed, duplex;
 	int i;
 
-	arch_get_mdio_control(netdev->name);
+	arch_get_mdio_control(name);
 #ifdef CONFIG_MACB_SEARCH_PHY
 	/* Auto-detect phy_addr */
 	if (!macb_phy_find(macb))
@@ -462,13 +477,13 @@ static int macb_phy_init(struct macb_device *macb)
 	/* Check if the PHY is up to snuff... */
 	phy_id = macb_mdio_read(macb, MII_PHYSID1);
 	if (phy_id == 0xffff) {
-		printf("%s: No PHY present\n", netdev->name);
+		printf("%s: No PHY present\n", name);
 		return 0;
 	}
 
 #ifdef CONFIG_PHYLIB
 	/* need to consider other phy interface mode */
-	phydev = phy_connect(macb->bus, macb->phy_addr, netdev,
+	phydev = phy_connect(macb->bus, macb->phy_addr, &macb->netdev,
 			     PHY_INTERFACE_MODE_RGMII);
 	if (!phydev) {
 		printf("phy_connect failed\n");
@@ -481,7 +496,7 @@ static int macb_phy_init(struct macb_device *macb)
 	status = macb_mdio_read(macb, MII_BMSR);
 	if (!(status & BMSR_LSTATUS)) {
 		/* Try to re-negotiate if we don't have link already. */
-		macb_phy_reset(macb);
+		macb_phy_reset(macb, name);
 
 		for (i = 0; i < MACB_AUTONEG_TIMEOUT / 100; i++) {
 			status = macb_mdio_read(macb, MII_BMSR);
@@ -493,7 +508,7 @@ static int macb_phy_init(struct macb_device *macb)
 
 	if (!(status & BMSR_LSTATUS)) {
 		printf("%s: link down (status: 0x%04x)\n",
-		       netdev->name, status);
+		       name, status);
 		return 0;
 	}
 
@@ -505,7 +520,7 @@ static int macb_phy_init(struct macb_device *macb)
 			duplex = ((lpa & LPA_1000FULL) ? 1 : 0);
 
 			printf("%s: link up, 1000Mbps %s-duplex (lpa: 0x%04x)\n",
-			       netdev->name,
+			       name,
 			       duplex ? "full" : "half",
 			       lpa);
 
@@ -530,7 +545,7 @@ static int macb_phy_init(struct macb_device *macb)
 		 ? 1 : 0);
 	duplex = (media & ADVERTISE_FULL) ? 1 : 0;
 	printf("%s: link up, %sMbps %s-duplex (lpa: 0x%04x)\n",
-	       netdev->name,
+	       name,
 	       speed ? "100" : "10",
 	       duplex ? "full" : "half",
 	       lpa);
@@ -570,9 +585,8 @@ static int gmac_init_multi_queues(struct macb_device *macb)
 	return 0;
 }
 
-static int macb_init(struct eth_device *netdev, bd_t *bd)
+static int _macb_init(struct macb_device *macb, const char *name)
 {
-	struct macb_device *macb = to_macb(netdev);
 	unsigned long paddr;
 	int i;
 
@@ -605,6 +619,7 @@ static int macb_init(struct eth_device *netdev, bd_t *bd)
 	macb->rx_tail = 0;
 	macb->tx_head = 0;
 	macb->tx_tail = 0;
+	macb->next_rx_tail = 0;
 
 	macb_writel(macb, RBQP, macb->rx_ring_dma);
 	macb_writel(macb, TBQP, macb->tx_ring_dma);
@@ -641,7 +656,7 @@ static int macb_init(struct eth_device *netdev, bd_t *bd)
 #endif /* CONFIG_RMII */
 	}
 
-	if (!macb_phy_init(macb))
+	if (!macb_phy_init(macb, name))
 		return -1;
 
 	/* Enable TX and RX */
@@ -650,9 +665,8 @@ static int macb_init(struct eth_device *netdev, bd_t *bd)
 	return 0;
 }
 
-static void macb_halt(struct eth_device *netdev)
+static void _macb_halt(struct macb_device *macb)
 {
-	struct macb_device *macb = to_macb(netdev);
 	u32 ncr, tsr;
 
 	/* Halt the controller and wait for any ongoing transmission to end. */
@@ -668,17 +682,16 @@ static void macb_halt(struct eth_device *netdev)
 	macb_writel(macb, NCR, MACB_BIT(CLRSTAT));
 }
 
-static int macb_write_hwaddr(struct eth_device *dev)
+static int _macb_write_hwaddr(struct macb_device *macb, unsigned char *enetaddr)
 {
-	struct macb_device *macb = to_macb(dev);
 	u32 hwaddr_bottom;
 	u16 hwaddr_top;
 
 	/* set hardware address */
-	hwaddr_bottom = dev->enetaddr[0] | dev->enetaddr[1] << 8 |
-			dev->enetaddr[2] << 16 | dev->enetaddr[3] << 24;
+	hwaddr_bottom = enetaddr[0] | enetaddr[1] << 8 |
+			enetaddr[2] << 16 | enetaddr[3] << 24;
 	macb_writel(macb, SA1B, hwaddr_bottom);
-	hwaddr_top = dev->enetaddr[4] | dev->enetaddr[5] << 8;
+	hwaddr_top = enetaddr[4] | enetaddr[5] << 8;
 	macb_writel(macb, SA1T, hwaddr_top);
 	return 0;
 }
@@ -739,11 +752,87 @@ static u32 macb_dbw(struct macb_device *macb)
 	}
 }
 
+static void _macb_eth_initialize(struct macb_device *macb)
+{
+	int id = 0;	/* This is not used by functions we call */
+	u32 ncfgr;
+
+	/* TODO: we need check the rx/tx_ring_dma is dcache line aligned */
+	macb->rx_buffer = dma_alloc_coherent(MACB_RX_BUFFER_SIZE,
+					     &macb->rx_buffer_dma);
+	macb->rx_ring = dma_alloc_coherent(MACB_RX_DMA_DESC_SIZE,
+					   &macb->rx_ring_dma);
+	macb->tx_ring = dma_alloc_coherent(MACB_TX_DMA_DESC_SIZE,
+					   &macb->tx_ring_dma);
+	macb->dummy_desc = dma_alloc_coherent(MACB_TX_DUMMY_DMA_DESC_SIZE,
+					   &macb->dummy_desc_dma);
+
+	/*
+	 * Do some basic initialization so that we at least can talk
+	 * to the PHY
+	 */
+	if (macb_is_gem(macb)) {
+		ncfgr = gem_mdc_clk_div(id, macb);
+		ncfgr |= macb_dbw(macb);
+	} else {
+		ncfgr = macb_mdc_clk_div(id, macb);
+	}
+
+	macb_writel(macb, NCFGR, ncfgr);
+}
+
+#ifndef CONFIG_DM_ETH
+static int macb_send(struct eth_device *netdev, void *packet, int length)
+{
+	struct macb_device *macb = to_macb(netdev);
+
+	return _macb_send(macb, netdev->name, packet, length);
+}
+
+static int macb_recv(struct eth_device *netdev)
+{
+	struct macb_device *macb = to_macb(netdev);
+	uchar *packet;
+	int length;
+
+	macb->wrapped = false;
+	for (;;) {
+		macb->next_rx_tail = macb->rx_tail;
+		length = _macb_recv(macb, &packet);
+		if (length >= 0) {
+			net_process_received_packet(packet, length);
+			reclaim_rx_buffers(macb, macb->next_rx_tail);
+		} else if (length < 0) {
+			return length;
+		}
+	}
+}
+
+static int macb_init(struct eth_device *netdev, bd_t *bd)
+{
+	struct macb_device *macb = to_macb(netdev);
+
+	return _macb_init(macb, netdev->name);
+}
+
+static void macb_halt(struct eth_device *netdev)
+{
+	struct macb_device *macb = to_macb(netdev);
+
+	return _macb_halt(macb);
+}
+
+static int macb_write_hwaddr(struct eth_device *netdev)
+{
+	struct macb_device *macb = to_macb(netdev);
+
+	return _macb_write_hwaddr(macb, netdev->enetaddr);
+}
+
 int macb_eth_initialize(int id, void *regs, unsigned int phy_addr)
 {
 	struct macb_device *macb;
 	struct eth_device *netdev;
-	u32 ncfgr;
 
 	macb = malloc(sizeof(struct macb_device));
 	if (!macb) {
@@ -754,17 +843,6 @@ int macb_eth_initialize(int id, void *regs, unsigned int phy_addr)
 
 	netdev = &macb->netdev;
 
-	macb->rx_buffer = dma_alloc_coherent(MACB_RX_BUFFER_SIZE,
-					     &macb->rx_buffer_dma);
-	macb->rx_ring = dma_alloc_coherent(MACB_RX_DMA_DESC_SIZE,
-					   &macb->rx_ring_dma);
-	macb->tx_ring = dma_alloc_coherent(MACB_TX_DMA_DESC_SIZE,
-					   &macb->tx_ring_dma);
-	macb->dummy_desc = dma_alloc_coherent(MACB_TX_DUMMY_DMA_DESC_SIZE,
-					   &macb->dummy_desc_dma);
-
-	/* TODO: we need check the rx/tx_ring_dma is dcache line aligned */
-
 	macb->regs = regs;
 	macb->phy_addr = phy_addr;
 
@@ -779,18 +857,7 @@ int macb_eth_initialize(int id, void *regs, unsigned int phy_addr)
 	netdev->recv = macb_recv;
 	netdev->write_hwaddr = macb_write_hwaddr;
 
-	/*
-	 * Do some basic initialization so that we at least can talk
-	 * to the PHY
-	 */
-	if (macb_is_gem(macb)) {
-		ncfgr = gem_mdc_clk_div(id, macb);
-		ncfgr |= macb_dbw(macb);
-	} else {
-		ncfgr = macb_mdc_clk_div(id, macb);
-	}
-
-	macb_writel(macb, NCFGR, ncfgr);
+	_macb_eth_initialize(macb);
 
 	eth_register(netdev);
 
@@ -800,5 +867,106 @@ int macb_eth_initialize(int id, void *regs, unsigned int phy_addr)
 #endif
 	return 0;
 }
+#endif /* !CONFIG_DM_ETH */
+
+#ifdef CONFIG_DM_ETH
+
+static int macb_start(struct udevice *dev)
+{
+	struct macb_device *macb = dev_get_priv(dev);
+
+	return _macb_init(macb, dev->name);
+}
+
+static int macb_send(struct udevice *dev, void *packet, int length)
+{
+	struct macb_device *macb = dev_get_priv(dev);
+
+	return _macb_send(macb, dev->name, packet, length);
+}
+
+static int macb_recv(struct udevice *dev, int flags, uchar **packetp)
+{
+	struct macb_device *macb = dev_get_priv(dev);
+
+	macb->next_rx_tail = macb->rx_tail;
+	macb->wrapped = false;
+
+	return _macb_recv(macb, packetp);
+}
+
+static int macb_free_pkt(struct udevice *dev, uchar *packet, int length)
+{
+	struct macb_device *macb = dev_get_priv(dev);
+
+	reclaim_rx_buffers(macb, macb->next_rx_tail);
+
+	return 0;
+}
+
+static void macb_stop(struct udevice *dev)
+{
+	struct macb_device *macb = dev_get_priv(dev);
+
+	_macb_halt(macb);
+}
+
+static int macb_write_hwaddr(struct udevice *dev)
+{
+	struct eth_pdata *plat = dev_get_platdata(dev);
+	struct macb_device *macb = dev_get_priv(dev);
+
+	return _macb_write_hwaddr(macb, plat->enetaddr);
+}
+
+static const struct eth_ops macb_eth_ops = {
+	.start	= macb_start,
+	.send	= macb_send,
+	.recv	= macb_recv,
+	.stop	= macb_stop,
+	.free_pkt	= macb_free_pkt,
+	.write_hwaddr	= macb_write_hwaddr,
+};
+
+static int macb_eth_probe(struct udevice *dev)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+	struct macb_device *macb = dev_get_priv(dev);
+
+	macb->regs = (void *)pdata->iobase;
+
+	_macb_eth_initialize(macb);
+#if defined(CONFIG_CMD_MII) || defined(CONFIG_PHYLIB)
+	miiphy_register(dev->name, macb_miiphy_read, macb_miiphy_write);
+	macb->bus = miiphy_get_dev_by_name(dev->name);
+#endif
+
+	return 0;
+}
+
+static int macb_eth_ofdata_to_platdata(struct udevice *dev)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+
+	pdata->iobase = dev_get_addr(dev);
+	return 0;
+}
+
+static const struct udevice_id macb_eth_ids[] = {
+	{ .compatible = "cdns,macb" },
+	{ }
+};
+
+U_BOOT_DRIVER(eth_macb) = {
+	.name	= "eth_macb",
+	.id	= UCLASS_ETH,
+	.of_match = macb_eth_ids,
+	.ofdata_to_platdata = macb_eth_ofdata_to_platdata,
+	.probe	= macb_eth_probe,
+	.ops	= &macb_eth_ops,
+	.priv_auto_alloc_size = sizeof(struct macb_device),
+	.platdata_auto_alloc_size = sizeof(struct eth_pdata),
+};
+#endif
 
 #endif
diff --git a/drivers/serial/atmel_usart.c b/drivers/serial/atmel_usart.c
index 4fe992bf2b..e450135c75 100644
--- a/drivers/serial/atmel_usart.c
+++ b/drivers/serial/atmel_usart.c
@@ -191,16 +191,35 @@ static int atmel_serial_probe(struct udevice *dev)
 {
 	struct atmel_serial_platdata *plat = dev->platdata;
 	struct atmel_serial_priv *priv = dev_get_priv(dev);
+#if CONFIG_IS_ENABLED(OF_CONTROL)
+	fdt_addr_t addr_base;
 
+	addr_base = dev_get_addr(dev);
+	if (addr_base == FDT_ADDR_T_NONE)
+		return -ENODEV;
+
+	plat->base_addr = (uint32_t)addr_base;
+#endif
 	priv->usart = (atmel_usart3_t *)plat->base_addr;
 	atmel_serial_init_internal(priv->usart);
 
 	return 0;
 }
 
+#if CONFIG_IS_ENABLED(OF_CONTROL)
+static const struct udevice_id atmel_serial_ids[] = {
+	{ .compatible = "atmel,at91sam9260-usart" },
+	{ }
+};
+#endif
+
 U_BOOT_DRIVER(serial_atmel) = {
 	.name	= "serial_atmel",
 	.id	= UCLASS_SERIAL,
+#if CONFIG_IS_ENABLED(OF_CONTROL)
+	.of_match = atmel_serial_ids,
+	.platdata_auto_alloc_size = sizeof(struct atmel_serial_platdata),
+#endif
 	.probe = atmel_serial_probe,
 	.ops	= &atmel_serial_ops,
 	.flags = DM_FLAG_PRE_RELOC,
diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index d43d8a59d3..39cd7caff1 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -7,6 +7,10 @@
  */
 
 #include <common.h>
+#include <atmel_lcd.h>
+#include <dm.h>
+#include <fdtdec.h>
+#include <video.h>
 #include <asm/io.h>
 #include <asm/arch/gpio.h>
 #include <asm/arch/clk.h>
@@ -14,6 +18,21 @@
 #include <bmp_layout.h>
 #include <atmel_lcdc.h>
 
+DECLARE_GLOBAL_DATA_PTR;
+
+#ifdef CONFIG_DM_VIDEO
+enum {
+	/* Maximum LCD size we support */
+	LCD_MAX_WIDTH		= 1366,
+	LCD_MAX_HEIGHT		= 768,
+	LCD_MAX_LOG2_BPP	= VIDEO_BPP16,
+};
+#endif
+
+struct atmel_fb_priv {
+	struct display_timing timing;
+};
+
 /* configurable parameters */
 #define ATMEL_LCDC_CVAL_DEFAULT		0xc8
 #define ATMEL_LCDC_DMA_BURST_LEN	8
@@ -30,6 +49,7 @@
 #define lcdc_readl(mmio, reg)		__raw_readl((mmio)+(reg))
 #define lcdc_writel(mmio, reg, val)	__raw_writel((val), (mmio)+(reg))
 
+#ifndef CONFIG_DM_VIDEO
 ushort *configuration_get_cmap(void)
 {
 	return (ushort *)(panel_info.mmio + ATMEL_LCDC_LUT(0));
@@ -90,40 +110,43 @@ void lcd_set_cmap(struct bmp_image *bmp, unsigned colors)
 		lcd_setcolreg(i, cte.red, cte.green, cte.blue);
 	}
 }
+#endif
 
-void lcd_ctrl_init(void *lcdbase)
+static void atmel_fb_init(ulong addr, struct display_timing *timing, int bpix,
+			  bool tft, bool cont_pol_low, ulong lcdbase)
 {
 	unsigned long value;
+	void *reg = (void *)addr;
 
 	/* Turn off the LCD controller and the DMA controller */
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_PWRCON,
+	lcdc_writel(reg, ATMEL_LCDC_PWRCON,
 		    ATMEL_LCDC_GUARD_TIME << ATMEL_LCDC_GUARDT_OFFSET);
 
 	/* Wait for the LCDC core to become idle */
-	while (lcdc_readl(panel_info.mmio, ATMEL_LCDC_PWRCON) & ATMEL_LCDC_BUSY)
+	while (lcdc_readl(reg, ATMEL_LCDC_PWRCON) & ATMEL_LCDC_BUSY)
 		udelay(10);
 
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_DMACON, 0);
+	lcdc_writel(reg, ATMEL_LCDC_DMACON, 0);
 
 	/* Reset LCDC DMA */
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_DMACON, ATMEL_LCDC_DMARST);
+	lcdc_writel(reg, ATMEL_LCDC_DMACON, ATMEL_LCDC_DMARST);
 
 	/* ...set frame size and burst length = 8 words (?) */
-	value = (panel_info.vl_col * panel_info.vl_row *
-		 NBITS(panel_info.vl_bpix)) / 32;
+	value = (timing->hactive.typ * timing->vactive.typ *
+		 (1 << bpix)) / 32;
 	value |= ((ATMEL_LCDC_DMA_BURST_LEN - 1) << ATMEL_LCDC_BLENGTH_OFFSET);
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_DMAFRMCFG, value);
+	lcdc_writel(reg, ATMEL_LCDC_DMAFRMCFG, value);
 
 	/* Set pixel clock */
-	value = get_lcdc_clk_rate(0) / panel_info.vl_clk;
-	if (get_lcdc_clk_rate(0) % panel_info.vl_clk)
+	value = get_lcdc_clk_rate(0) / timing->pixelclock.typ;
+	if (get_lcdc_clk_rate(0) % timing->pixelclock.typ)
 		value++;
 	value = (value / 2) - 1;
 
 	if (!value) {
-		lcdc_writel(panel_info.mmio, ATMEL_LCDC_LCDCON1, ATMEL_LCDC_BYPASS);
+		lcdc_writel(reg, ATMEL_LCDC_LCDCON1, ATMEL_LCDC_BYPASS);
 	} else
-		lcdc_writel(panel_info.mmio, ATMEL_LCDC_LCDCON1,
+		lcdc_writel(reg, ATMEL_LCDC_LCDCON1,
 			    value << ATMEL_LCDC_CLKVAL_OFFSET);
 
 	/* Initialize control register 2 */
@@ -132,58 +155,160 @@ void lcd_ctrl_init(void *lcdbase)
 #else
 	value = ATMEL_LCDC_MEMOR_LITTLE | ATMEL_LCDC_CLKMOD_ALWAYSACTIVE;
 #endif
-	if (panel_info.vl_tft)
+	if (tft)
 		value |= ATMEL_LCDC_DISTYPE_TFT;
 
-	value |= panel_info.vl_sync;
-	value |= (panel_info.vl_bpix << 5);
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_LCDCON2, value);
+	if (!(timing->flags & DISPLAY_FLAGS_HSYNC_HIGH))
+		value |= ATMEL_LCDC_INVLINE_INVERTED;
+	if (!(timing->flags & DISPLAY_FLAGS_VSYNC_HIGH))
+		value |= ATMEL_LCDC_INVFRAME_INVERTED;
+	value |= bpix << 5;
+	lcdc_writel(reg, ATMEL_LCDC_LCDCON2, value);
 
 	/* Vertical timing */
-	value = (panel_info.vl_vsync_len - 1) << ATMEL_LCDC_VPW_OFFSET;
-	value |= panel_info.vl_upper_margin << ATMEL_LCDC_VBP_OFFSET;
-	value |= panel_info.vl_lower_margin;
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_TIM1, value);
+	value = (timing->vsync_len.typ - 1) << ATMEL_LCDC_VPW_OFFSET;
+	value |= timing->vback_porch.typ << ATMEL_LCDC_VBP_OFFSET;
+	value |= timing->vfront_porch.typ;
+	/* Magic! (Datasheet says "Bit 31 must be written to 1") */
+	value |= 1U << 31;
+	lcdc_writel(reg, ATMEL_LCDC_TIM1, value);
 
 	/* Horizontal timing */
-	value = (panel_info.vl_right_margin - 1) << ATMEL_LCDC_HFP_OFFSET;
-	value |= (panel_info.vl_hsync_len - 1) << ATMEL_LCDC_HPW_OFFSET;
-	value |= (panel_info.vl_left_margin - 1);
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_TIM2, value);
+	value = (timing->hfront_porch.typ - 1) << ATMEL_LCDC_HFP_OFFSET;
+	value |= (timing->hsync_len.typ - 1) << ATMEL_LCDC_HPW_OFFSET;
+	value |= (timing->hback_porch.typ - 1);
+	lcdc_writel(reg, ATMEL_LCDC_TIM2, value);
 
 	/* Display size */
-	value = (panel_info.vl_col - 1) << ATMEL_LCDC_HOZVAL_OFFSET;
-	value |= panel_info.vl_row - 1;
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_LCDFRMCFG, value);
+	value = (timing->hactive.typ - 1) << ATMEL_LCDC_HOZVAL_OFFSET;
+	value |= timing->vactive.typ - 1;
+	lcdc_writel(reg, ATMEL_LCDC_LCDFRMCFG, value);
 
 	/* FIFO Threshold: Use formula from data sheet */
 	value = ATMEL_LCDC_FIFO_SIZE - (2 * ATMEL_LCDC_DMA_BURST_LEN + 3);
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_FIFO, value);
+	lcdc_writel(reg, ATMEL_LCDC_FIFO, value);
 
 	/* Toggle LCD_MODE every frame */
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_MVAL, 0);
+	lcdc_writel(reg, ATMEL_LCDC_MVAL, 0);
 
 	/* Disable all interrupts */
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_IDR, ~0UL);
+	lcdc_writel(reg, ATMEL_LCDC_IDR, ~0UL);
 
 	/* Set contrast */
 	value = ATMEL_LCDC_PS_DIV8 |
 		ATMEL_LCDC_ENA_PWMENABLE;
-	if (!panel_info.vl_cont_pol_low)
+	if (!cont_pol_low)
 		value |= ATMEL_LCDC_POL_POSITIVE;
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_CONTRAST_CTR, value);
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_CONTRAST_VAL, ATMEL_LCDC_CVAL_DEFAULT);
+	lcdc_writel(reg, ATMEL_LCDC_CONTRAST_CTR, value);
+	lcdc_writel(reg, ATMEL_LCDC_CONTRAST_VAL, ATMEL_LCDC_CVAL_DEFAULT);
 
 	/* Set framebuffer DMA base address and pixel offset */
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_DMABADDR1, (u_long)lcdbase);
+	lcdc_writel(reg, ATMEL_LCDC_DMABADDR1, lcdbase);
 
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_DMACON, ATMEL_LCDC_DMAEN);
-	lcdc_writel(panel_info.mmio, ATMEL_LCDC_PWRCON,
+	lcdc_writel(reg, ATMEL_LCDC_DMACON, ATMEL_LCDC_DMAEN);
+	lcdc_writel(reg, ATMEL_LCDC_PWRCON,
 		    (ATMEL_LCDC_GUARD_TIME << ATMEL_LCDC_GUARDT_OFFSET) | ATMEL_LCDC_PWR);
 }
 
+#ifndef CONFIG_DM_VIDEO
+void lcd_ctrl_init(void *lcdbase)
+{
+	struct display_timing timing;
+
+	timing.flags = 0;
+	if (!(panel_info.vl_sync & ATMEL_LCDC_INVLINE_INVERTED))
+		timing.flags |= DISPLAY_FLAGS_HSYNC_HIGH;
+	if (!(panel_info.vl_sync & ATMEL_LCDC_INVFRAME_INVERTED))
+		timing.flags |= DISPLAY_FLAGS_VSYNC_LOW;
+	timing.pixelclock.typ = panel_info.vl_clk;
+
+	timing.hactive.typ = panel_info.vl_col;
+	timing.hfront_porch.typ = panel_info.vl_right_margin;
+	timing.hback_porch.typ = panel_info.vl_left_margin;
+	timing.hsync_len.typ = panel_info.vl_hsync_len;
+
+	timing.vactive.typ = panel_info.vl_row;
+	timing.vfront_porch.typ = panel_info.vl_clk;
+	timing.vback_porch.typ = panel_info.vl_clk;
+	timing.vsync_len.typ = panel_info.vl_clk;
+
+	atmel_fb_init(panel_info.mmio, &timing, panel_info.vl_bpix,
+		      panel_info.vl_tft, panel_info.vl_cont_pol_low,
+		      (ulong)lcdbase);
+}
+
 ulong calc_fbsize(void)
 {
 	return ((panel_info.vl_col * panel_info.vl_row *
 		NBITS(panel_info.vl_bpix)) / 8) + PAGE_SIZE;
 }
+#endif
+
+#ifdef CONFIG_DM_VIDEO
+static int atmel_fb_lcd_probe(struct udevice *dev)
+{
+	struct video_uc_platdata *uc_plat = dev_get_uclass_platdata(dev);
+	struct video_priv *uc_priv = dev_get_uclass_priv(dev);
+	struct atmel_fb_priv *priv = dev_get_priv(dev);
+	struct display_timing *timing = &priv->timing;
+
+	/*
+	 * For now some values are hard-coded. We could use the device tree
+	 * bindings in simple-framebuffer.txt to specify the format/bpp and
+	 * some Atmel-specific binding for tft and cont_pol_low.
+	 */
+	atmel_fb_init(ATMEL_BASE_LCDC, timing, VIDEO_BPP16, true, false,
+		      uc_plat->base);
+	uc_priv->xsize = timing->hactive.typ;
+	uc_priv->ysize = timing->vactive.typ;
+	uc_priv->bpix = VIDEO_BPP16;
+	video_set_flush_dcache(dev, true);
+	debug("LCD frame buffer at %lx, size %x, %dx%d pixels\n", uc_plat->base,
+	      uc_plat->size, uc_priv->xsize, uc_priv->ysize);
+
+	return 0;
+}
+
+static int atmel_fb_ofdata_to_platdata(struct udevice *dev)
+{
+	struct atmel_lcd_platdata *plat = dev_get_platdata(dev);
+	struct atmel_fb_priv *priv = dev_get_priv(dev);
+	struct display_timing *timing = &priv->timing;
+	const void *blob = gd->fdt_blob;
+
+	if (fdtdec_decode_display_timing(blob, dev->of_offset,
+					 plat->timing_index, timing)) {
+		debug("%s: Failed to decode display timing\n", __func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int atmel_fb_lcd_bind(struct udevice *dev)
+{
+	struct video_uc_platdata *uc_plat = dev_get_uclass_platdata(dev);
+
+	uc_plat->size = LCD_MAX_WIDTH * LCD_MAX_HEIGHT *
+			(1 << VIDEO_BPP16) / 8;
+	debug("%s: Frame buffer size %x\n", __func__, uc_plat->size);
+
+	return 0;
+}
+
+static const struct udevice_id atmel_fb_lcd_ids[] = {
+	{ .compatible = "atmel,at91sam9g45-lcdc" },
+	{ }
+};
+
+U_BOOT_DRIVER(atmel_fb) = {
+	.name	= "atmel_fb",
+	.id	= UCLASS_VIDEO,
+	.of_match = atmel_fb_lcd_ids,
+	.bind	= atmel_fb_lcd_bind,
+	.ofdata_to_platdata	= atmel_fb_ofdata_to_platdata,
+	.probe	= atmel_fb_lcd_probe,
+	.platdata_auto_alloc_size = sizeof(struct atmel_lcd_platdata),
+	.priv_auto_alloc_size	= sizeof(struct atmel_fb_priv),
+};
+#endif