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

9 files changed, 1306 insertions, 128 deletions

diff --git a/drivers/i2c/Makefile b/drivers/i2c/Makefile
index fa3a875..36d5e5f 100644
--- a/drivers/i2c/Makefile
+++ b/drivers/i2c/Makefile
@@ -18,6 +18,7 @@ obj-$(CONFIG_SH_SH7734_I2C) += sh_sh7734_i2c.o
 obj-$(CONFIG_SYS_I2C) += i2c_core.o
 obj-$(CONFIG_SYS_I2C_FSL) += fsl_i2c.o
 obj-$(CONFIG_SYS_I2C_FTI2C010) += fti2c010.o
+obj-$(CONFIG_SYS_I2C_KONA) += kona_i2c.o
 obj-$(CONFIG_SYS_I2C_MXC) += mxc_i2c.o
 obj-$(CONFIG_SYS_I2C_OMAP24XX) += omap24xx_i2c.o
 obj-$(CONFIG_SYS_I2C_OMAP34XX) += omap24xx_i2c.o
diff --git a/drivers/i2c/adi_i2c.c b/drivers/i2c/adi_i2c.c
new file mode 100644
index 0000000..675f417
--- /dev/null
+++ b/drivers/i2c/adi_i2c.c
@@ -0,0 +1,387 @@
+/*
+ * i2c.c - driver for ADI TWI/I2C
+ *
+ * Copyright (c) 2006-2013 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <common.h>
+#include <i2c.h>
+
+#include <asm/clock.h>
+#include <asm/twi.h>
+#include <asm/io.h>
+
+/* Every register is 32bit aligned, but only 16bits in size */
+#define ureg(name) u16 name; u16 __pad_##name;
+struct twi_regs {
+	ureg(clkdiv);
+	ureg(control);
+	ureg(slave_ctl);
+	ureg(slave_stat);
+	ureg(slave_addr);
+	ureg(master_ctl);
+	ureg(master_stat);
+	ureg(master_addr);
+	ureg(int_stat);
+	ureg(int_mask);
+	ureg(fifo_ctl);
+	ureg(fifo_stat);
+	char __pad[0x50];
+	ureg(xmt_data8);
+	ureg(xmt_data16);
+	ureg(rcv_data8);
+	ureg(rcv_data16);
+};
+#undef ureg
+
+/* U-Boot I2C framework allows only one active device at a time.  */
+#ifdef TWI_CLKDIV
+#define TWI0_CLKDIV TWI_CLKDIV
+#endif
+static struct twi_regs *twi = (void *)TWI0_CLKDIV;
+
+#ifdef DEBUG
+# define dmemset(s, c, n) memset(s, c, n)
+#else
+# define dmemset(s, c, n)
+#endif
+#define debugi(fmt, args...) \
+	debug( \
+		"MSTAT:0x%03x FSTAT:0x%x ISTAT:0x%02x\t%-20s:%-3i: " fmt "\n", \
+		twi->master_stat, twi->fifo_stat, twi->int_stat, \
+		__func__, __LINE__, ## args)
+
+#ifdef CONFIG_TWICLK_KHZ
+# error do not define CONFIG_TWICLK_KHZ ... use CONFIG_SYS_I2C_SPEED
+#endif
+
+/*
+ * The way speed is changed into duty often results in integer truncation
+ * with 50% duty, so we'll force rounding up to the next duty by adding 1
+ * to the max.  In practice this will get us a speed of something like
+ * 385 KHz.  The other limit is easy to handle as it is only 8 bits.
+ */
+#define I2C_SPEED_MAX             400000
+#define I2C_SPEED_TO_DUTY(speed)  (5000000 / (speed))
+#define I2C_DUTY_MAX              (I2C_SPEED_TO_DUTY(I2C_SPEED_MAX) + 1)
+#define I2C_DUTY_MIN              0xff	/* 8 bit limited */
+#define SYS_I2C_DUTY              I2C_SPEED_TO_DUTY(CONFIG_SYS_I2C_SPEED)
+/* Note: duty is inverse of speed, so the comparisons below are correct */
+#if SYS_I2C_DUTY < I2C_DUTY_MAX || SYS_I2C_DUTY > I2C_DUTY_MIN
+# error "The Blackfin I2C hardware can only operate 20KHz - 400KHz"
+#endif
+
+/* All transfers are described by this data structure */
+struct i2c_msg {
+	u8 flags;
+#define I2C_M_COMBO		0x4
+#define I2C_M_STOP		0x2
+#define I2C_M_READ		0x1
+	int len;		/* msg length */
+	u8 *buf;		/* pointer to msg data */
+	int alen;		/* addr length */
+	u8 *abuf;		/* addr buffer */
+};
+
+/* Allow msec timeout per ~byte transfer */
+#define I2C_TIMEOUT 10
+
+/**
+ * wait_for_completion - manage the actual i2c transfer
+ *	@msg: the i2c msg
+ */
+static int wait_for_completion(struct i2c_msg *msg)
+{
+	u16 int_stat, ctl;
+	ulong timebase = get_timer(0);
+
+	do {
+		int_stat = readw(&twi->int_stat);
+
+		if (int_stat & XMTSERV) {
+			debugi("processing XMTSERV");
+			writew(XMTSERV, &twi->int_stat);
+			if (msg->alen) {
+				writew(*(msg->abuf++), &twi->xmt_data8);
+				--msg->alen;
+			} else if (!(msg->flags & I2C_M_COMBO) && msg->len) {
+				writew(*(msg->buf++), &twi->xmt_data8);
+				--msg->len;
+			} else {
+				ctl = readw(&twi->master_ctl);
+				if (msg->flags & I2C_M_COMBO)
+					writew(ctl | RSTART | MDIR,
+							&twi->master_ctl);
+				else
+					writew(ctl | STOP, &twi->master_ctl);
+			}
+		}
+		if (int_stat & RCVSERV) {
+			debugi("processing RCVSERV");
+			writew(RCVSERV, &twi->int_stat);
+			if (msg->len) {
+				*(msg->buf++) = readw(&twi->rcv_data8);
+				--msg->len;
+			} else if (msg->flags & I2C_M_STOP) {
+				ctl = readw(&twi->master_ctl);
+				writew(ctl | STOP, &twi->master_ctl);
+			}
+		}
+		if (int_stat & MERR) {
+			debugi("processing MERR");
+			writew(MERR, &twi->int_stat);
+			return msg->len;
+		}
+		if (int_stat & MCOMP) {
+			debugi("processing MCOMP");
+			writew(MCOMP, &twi->int_stat);
+			if (msg->flags & I2C_M_COMBO && msg->len) {
+				ctl = readw(&twi->master_ctl);
+				ctl = (ctl & ~RSTART) |
+					(min(msg->len, 0xff) << 6) | MEN | MDIR;
+				writew(ctl, &twi->master_ctl);
+			} else
+				break;
+		}
+
+		/* If we were able to do something, reset timeout */
+		if (int_stat)
+			timebase = get_timer(0);
+
+	} while (get_timer(timebase) < I2C_TIMEOUT);
+
+	return msg->len;
+}
+
+/**
+ * i2c_transfer - setup an i2c transfer
+ *	@return: 0 if things worked, non-0 if things failed
+ *
+ *	Here we just get the i2c stuff all prepped and ready, and then tail off
+ *	into wait_for_completion() for all the bits to go.
+ */
+static int i2c_transfer(uchar chip, uint addr, int alen, uchar *buffer,
+			int len, u8 flags)
+{
+	int ret;
+	u16 ctl;
+	uchar addr_buffer[] = {
+		(addr >>  0),
+		(addr >>  8),
+		(addr >> 16),
+	};
+	struct i2c_msg msg = {
+		.flags = flags | (len >= 0xff ? I2C_M_STOP : 0),
+		.buf   = buffer,
+		.len   = len,
+		.abuf  = addr_buffer,
+		.alen  = alen,
+	};
+
+	dmemset(buffer, 0xff, len);
+	debugi("chip=0x%x addr=0x%02x alen=%i buf[0]=0x%02x len=%i ",
+		chip, addr, alen, buffer[0], len);
+	debugi("flags=0x%02x[%s] ", flags,
+		(flags & I2C_M_READ ? "rd" : "wr"));
+
+	/* wait for things to settle */
+	while (readw(&twi->master_stat) & BUSBUSY)
+		if (ctrlc())
+			return 1;
+
+	/* Set Transmit device address */
+	writew(chip, &twi->master_addr);
+
+	/* Clear the FIFO before starting things */
+	writew(XMTFLUSH | RCVFLUSH, &twi->fifo_ctl);
+	writew(0, &twi->fifo_ctl);
+
+	/* prime the pump */
+	if (msg.alen) {
+		len = (msg.flags & I2C_M_COMBO) ? msg.alen : msg.alen + len;
+		debugi("first byte=0x%02x", *msg.abuf);
+		writew(*(msg.abuf++), &twi->xmt_data8);
+		--msg.alen;
+	} else if (!(msg.flags & I2C_M_READ) && msg.len) {
+		debugi("first byte=0x%02x", *msg.buf);
+		writew(*(msg.buf++), &twi->xmt_data8);
+		--msg.len;
+	}
+
+	/* clear int stat */
+	writew(-1, &twi->master_stat);
+	writew(-1, &twi->int_stat);
+	writew(0, &twi->int_mask);
+
+	/* Master enable */
+	ctl = readw(&twi->master_ctl);
+	ctl = (ctl & FAST) | (min(len, 0xff) << 6) | MEN |
+		((msg.flags & I2C_M_READ) ? MDIR : 0);
+	writew(ctl, &twi->master_ctl);
+
+	/* process the rest */
+	ret = wait_for_completion(&msg);
+	debugi("ret=%d", ret);
+
+	if (ret) {
+		ctl = readw(&twi->master_ctl) & ~MEN;
+		writew(ctl, &twi->master_ctl);
+		ctl = readw(&twi->control) & ~TWI_ENA;
+		writew(ctl, &twi->control);
+		ctl = readw(&twi->control) | TWI_ENA;
+		writew(ctl, &twi->control);
+	}
+
+	return ret;
+}
+
+/**
+ * i2c_set_bus_speed - set i2c bus speed
+ *	@speed: bus speed (in HZ)
+ */
+int i2c_set_bus_speed(unsigned int speed)
+{
+	u16 clkdiv = I2C_SPEED_TO_DUTY(speed);
+
+	/* Set TWI interface clock */
+	if (clkdiv < I2C_DUTY_MAX || clkdiv > I2C_DUTY_MIN)
+		return -1;
+	clkdiv = (clkdiv << 8) | (clkdiv & 0xff);
+	writew(clkdiv, &twi->clkdiv);
+
+	/* Don't turn it on */
+	writew(speed > 100000 ? FAST : 0, &twi->master_ctl);
+
+	return 0;
+}
+
+/**
+ * i2c_get_bus_speed - get i2c bus speed
+ *	@speed: bus speed (in HZ)
+ */
+unsigned int i2c_get_bus_speed(void)
+{
+	u16 clkdiv = readw(&twi->clkdiv) & 0xff;
+	/* 10 MHz / (2 * CLKDIV) -> 5 MHz / CLKDIV */
+	return 5000000 / clkdiv;
+}
+
+/**
+ * i2c_init - initialize the i2c bus
+ *	@speed: bus speed (in HZ)
+ *	@slaveaddr: address of device in slave mode (0 - not slave)
+ *
+ *	Slave mode isn't actually implemented.  It'll stay that way until
+ *	we get a real request for it.
+ */
+void i2c_init(int speed, int slaveaddr)
+{
+	u16 prescale = ((get_i2c_clk() / 1000 / 1000 + 5) / 10) & 0x7F;
+
+	/* Set TWI internal clock as 10MHz */
+	writew(prescale, &twi->control);
+
+	/* Set TWI interface clock as specified */
+	i2c_set_bus_speed(speed);
+
+	/* Enable it */
+	writew(TWI_ENA | prescale, &twi->control);
+
+	debugi("CONTROL:0x%04x CLKDIV:0x%04x", readw(&twi->control),
+		readw(&twi->clkdiv));
+
+#if CONFIG_SYS_I2C_SLAVE
+# error I2C slave support not tested/supported
+#endif
+}
+
+/**
+ * i2c_probe - test if a chip exists at a given i2c address
+ *	@chip: i2c chip addr to search for
+ *	@return: 0 if found, non-0 if not found
+ */
+int i2c_probe(uchar chip)
+{
+	u8 byte;
+	return i2c_read(chip, 0, 0, &byte, 1);
+}
+
+/**
+ * i2c_read - read data from an i2c device
+ *	@chip: i2c chip addr
+ *	@addr: memory (register) address in the chip
+ *	@alen: byte size of address
+ *	@buffer: buffer to store data read from chip
+ *	@len: how many bytes to read
+ *	@return: 0 on success, non-0 on failure
+ */
+int i2c_read(uchar chip, uint addr, int alen, uchar *buffer, int len)
+{
+	return i2c_transfer(chip, addr, alen, buffer,
+			len, (alen ? I2C_M_COMBO : I2C_M_READ));
+}
+
+/**
+ * i2c_write - write data to an i2c device
+ *	@chip: i2c chip addr
+ *	@addr: memory (register) address in the chip
+ *	@alen: byte size of address
+ *	@buffer: buffer holding data to write to chip
+ *	@len: how many bytes to write
+ *	@return: 0 on success, non-0 on failure
+ */
+int i2c_write(uchar chip, uint addr, int alen, uchar *buffer, int len)
+{
+	return i2c_transfer(chip, addr, alen, buffer, len, 0);
+}
+
+/**
+ * i2c_set_bus_num - change active I2C bus
+ *	@bus: bus index, zero based
+ *	@returns: 0 on success, non-0 on failure
+ */
+int i2c_set_bus_num(unsigned int bus)
+{
+	switch (bus) {
+#if CONFIG_SYS_MAX_I2C_BUS > 0
+	case 0:
+		twi = (void *)TWI0_CLKDIV;
+		return 0;
+#endif
+#if CONFIG_SYS_MAX_I2C_BUS > 1
+	case 1:
+		twi = (void *)TWI1_CLKDIV;
+		return 0;
+#endif
+#if CONFIG_SYS_MAX_I2C_BUS > 2
+	case 2:
+		twi = (void *)TWI2_CLKDIV;
+		return 0;
+#endif
+	default: return -1;
+	}
+}
+
+/**
+ * i2c_get_bus_num - returns index of active I2C bus
+ */
+unsigned int i2c_get_bus_num(void)
+{
+	switch ((unsigned long)twi) {
+#if CONFIG_SYS_MAX_I2C_BUS > 0
+	case TWI0_CLKDIV:
+		return 0;
+#endif
+#if CONFIG_SYS_MAX_I2C_BUS > 1
+	case TWI1_CLKDIV:
+		return 1;
+#endif
+#if CONFIG_SYS_MAX_I2C_BUS > 2
+	case TWI2_CLKDIV:
+		return 2;
+#endif
+	default: return -1;
+	}
+}
diff --git a/drivers/i2c/bfin-twi_i2c.c b/drivers/i2c/bfin-twi_i2c.c
index b3a04d3..cfab064 100644
--- a/drivers/i2c/bfin-twi_i2c.c
+++ b/drivers/i2c/bfin-twi_i2c.c
@@ -10,6 +10,7 @@
 #include <i2c.h>
 
 #include <asm/blackfin.h>
+#include <asm/clock.h>
 #include <asm/mach-common/bits/twi.h>
 
 /* Every register is 32bit aligned, but only 16bits in size */
@@ -274,7 +275,7 @@ unsigned int i2c_get_bus_speed(void)
  */
 void i2c_init(int speed, int slaveaddr)
 {
-	uint8_t prescale = ((get_sclk() / 1024 / 1024 + 5) / 10) & 0x7F;
+	uint8_t prescale = ((get_i2c_clk() / 1000 / 1000 + 5) / 10) & 0x7F;
 
 	/* Set TWI internal clock as 10MHz */
 	twi->control = prescale;
diff --git a/drivers/i2c/designware_i2c.c b/drivers/i2c/designware_i2c.c
index 9ed9295..c891ebd 100644
--- a/drivers/i2c/designware_i2c.c
+++ b/drivers/i2c/designware_i2c.c
@@ -7,7 +7,6 @@
 
 #include <common.h>
 #include <asm/io.h>
-#include <asm/arch/hardware.h>
 #include "designware_i2c.h"
 
 #ifdef CONFIG_I2C_MULTI_BUS
@@ -197,35 +196,18 @@ static int i2c_wait_for_bb(void)
 	return 0;
 }
 
-/* check parameters for i2c_read and i2c_write */
-static int check_params(uint addr, int alen, uchar *buffer, int len)
-{
-	if (buffer == NULL) {
-		printf("Buffer is invalid\n");
-		return 1;
-	}
-
-	if (alen > 1) {
-		printf("addr len %d not supported\n", alen);
-		return 1;
-	}
-
-	if (addr + len > 256) {
-		printf("address out of range\n");
-		return 1;
-	}
-
-	return 0;
-}
-
-static int i2c_xfer_init(uchar chip, uint addr)
+static int i2c_xfer_init(uchar chip, uint addr, int alen)
 {
 	if (i2c_wait_for_bb())
 		return 1;
 
 	i2c_setaddress(chip);
-	writel(addr, &i2c_regs_p->ic_cmd_data);
-
+	while (alen) {
+		alen--;
+		/* high byte address going out first */
+		writel((addr >> (alen * 8)) & 0xff,
+		       &i2c_regs_p->ic_cmd_data);
+	}
 	return 0;
 }
 
@@ -285,10 +267,7 @@ int i2c_read(uchar chip, uint addr, int alen, uchar *buffer, int len)
 	      addr);
 #endif
 
-	if (check_params(addr, alen, buffer, len))
-		return 1;
-
-	if (i2c_xfer_init(chip, addr))
+	if (i2c_xfer_init(chip, addr, alen))
 		return 1;
 
 	start_time_rx = get_timer(0);
@@ -345,10 +324,7 @@ int i2c_write(uchar chip, uint addr, int alen, uchar *buffer, int len)
 	      addr);
 #endif
 
-	if (check_params(addr, alen, buffer, len))
-		return 1;
-
-	if (i2c_xfer_init(chip, addr))
+	if (i2c_xfer_init(chip, addr, alen))
 		return 1;
 
 	start_time_tx = get_timer(0);
diff --git a/drivers/i2c/kona_i2c.c b/drivers/i2c/kona_i2c.c
new file mode 100644
index 0000000..0b1715a
--- /dev/null
+++ b/drivers/i2c/kona_i2c.c
@@ -0,0 +1,730 @@
+/*
+ * Copyright 2013 Broadcom Corporation.
+ *
+ * SPDX-License-Identifier:      GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <asm/arch/sysmap.h>
+#include <asm/kona-common/clk.h>
+#include <i2c.h>
+
+/* Hardware register offsets and field defintions */
+#define CS_OFFSET				0x00000020
+#define CS_ACK_SHIFT				3
+#define CS_ACK_MASK				0x00000008
+#define CS_ACK_CMD_GEN_START			0x00000000
+#define CS_ACK_CMD_GEN_RESTART			0x00000001
+#define CS_CMD_SHIFT				1
+#define CS_CMD_CMD_NO_ACTION			0x00000000
+#define CS_CMD_CMD_START_RESTART		0x00000001
+#define CS_CMD_CMD_STOP				0x00000002
+#define CS_EN_SHIFT				0
+#define CS_EN_CMD_ENABLE_BSC			0x00000001
+
+#define TIM_OFFSET				0x00000024
+#define TIM_PRESCALE_SHIFT			6
+#define TIM_P_SHIFT				3
+#define TIM_NO_DIV_SHIFT			2
+#define TIM_DIV_SHIFT				0
+
+#define DAT_OFFSET				0x00000028
+
+#define TOUT_OFFSET				0x0000002c
+
+#define TXFCR_OFFSET				0x0000003c
+#define TXFCR_FIFO_FLUSH_MASK			0x00000080
+#define TXFCR_FIFO_EN_MASK			0x00000040
+
+#define IER_OFFSET				0x00000044
+#define IER_READ_COMPLETE_INT_MASK		0x00000010
+#define IER_I2C_INT_EN_MASK			0x00000008
+#define IER_FIFO_INT_EN_MASK			0x00000002
+#define IER_NOACK_EN_MASK			0x00000001
+
+#define ISR_OFFSET				0x00000048
+#define ISR_RESERVED_MASK			0xffffff60
+#define ISR_CMDBUSY_MASK			0x00000080
+#define ISR_READ_COMPLETE_MASK			0x00000010
+#define ISR_SES_DONE_MASK			0x00000008
+#define ISR_ERR_MASK				0x00000004
+#define ISR_TXFIFOEMPTY_MASK			0x00000002
+#define ISR_NOACK_MASK				0x00000001
+
+#define CLKEN_OFFSET				0x0000004c
+#define CLKEN_AUTOSENSE_OFF_MASK		0x00000080
+#define CLKEN_M_SHIFT				4
+#define CLKEN_N_SHIFT				1
+#define CLKEN_CLKEN_MASK			0x00000001
+
+#define FIFO_STATUS_OFFSET			0x00000054
+#define FIFO_STATUS_RXFIFO_EMPTY_MASK		0x00000004
+#define FIFO_STATUS_TXFIFO_EMPTY_MASK		0x00000010
+
+#define HSTIM_OFFSET				0x00000058
+#define HSTIM_HS_MODE_MASK			0x00008000
+#define HSTIM_HS_HOLD_SHIFT			10
+#define HSTIM_HS_HIGH_PHASE_SHIFT		5
+#define HSTIM_HS_SETUP_SHIFT			0
+
+#define PADCTL_OFFSET				0x0000005c
+#define PADCTL_PAD_OUT_EN_MASK			0x00000004
+
+#define RXFCR_OFFSET				0x00000068
+#define RXFCR_NACK_EN_SHIFT			7
+#define RXFCR_READ_COUNT_SHIFT			0
+#define RXFIFORDOUT_OFFSET			0x0000006c
+
+/* Locally used constants */
+#define MAX_RX_FIFO_SIZE		64U	/* bytes */
+#define MAX_TX_FIFO_SIZE		64U	/* bytes */
+
+#define I2C_TIMEOUT			100000	/* usecs */
+
+#define WAIT_INT_CHK			100	/* usecs */
+#if I2C_TIMEOUT % WAIT_INT_CHK
+#error I2C_TIMEOUT must be a multiple of WAIT_INT_CHK
+#endif
+
+/* Operations that can be commanded to the controller */
+enum bcm_kona_cmd_t {
+	BCM_CMD_NOACTION = 0,
+	BCM_CMD_START,
+	BCM_CMD_RESTART,
+	BCM_CMD_STOP,
+};
+
+enum bus_speed_index {
+	BCM_SPD_100K = 0,
+	BCM_SPD_400K,
+	BCM_SPD_1MHZ,
+};
+
+/* Internal divider settings for standard mode, fast mode and fast mode plus */
+struct bus_speed_cfg {
+	uint8_t time_m;		/* Number of cycles for setup time */
+	uint8_t time_n;		/* Number of cycles for hold time */
+	uint8_t prescale;	/* Prescale divider */
+	uint8_t time_p;		/* Timing coefficient */
+	uint8_t no_div;		/* Disable clock divider */
+	uint8_t time_div;	/* Post-prescale divider */
+};
+
+static const struct bus_speed_cfg std_cfg_table[] = {
+	[BCM_SPD_100K] = {0x01, 0x01, 0x03, 0x06, 0x00, 0x02},
+	[BCM_SPD_400K] = {0x05, 0x01, 0x03, 0x05, 0x01, 0x02},
+	[BCM_SPD_1MHZ] = {0x01, 0x01, 0x03, 0x01, 0x01, 0x03},
+};
+
+struct bcm_kona_i2c_dev {
+	void *base;
+	uint speed;
+	const struct bus_speed_cfg *std_cfg;
+};
+
+/* Keep these two defines in sync */
+#define DEF_SPD 100000
+#define DEF_SPD_ENUM BCM_SPD_100K
+
+#define DEF_DEVICE(num) \
+{(void *)CONFIG_SYS_I2C_BASE##num, DEF_SPD, &std_cfg_table[DEF_SPD_ENUM]}
+
+static struct bcm_kona_i2c_dev g_i2c_devs[CONFIG_SYS_MAX_I2C_BUS] = {
+#ifdef CONFIG_SYS_I2C_BASE0
+	DEF_DEVICE(0),
+#endif
+#ifdef CONFIG_SYS_I2C_BASE1
+	DEF_DEVICE(1),
+#endif
+#ifdef CONFIG_SYS_I2C_BASE2
+	DEF_DEVICE(2),
+#endif
+#ifdef CONFIG_SYS_I2C_BASE3
+	DEF_DEVICE(3),
+#endif
+#ifdef CONFIG_SYS_I2C_BASE4
+	DEF_DEVICE(4),
+#endif
+#ifdef CONFIG_SYS_I2C_BASE5
+	DEF_DEVICE(5),
+#endif
+};
+
+#define I2C_M_TEN	0x0010	/* ten bit address */
+#define I2C_M_RD	0x0001	/* read data */
+#define I2C_M_NOSTART	0x4000	/* no restart between msgs */
+
+struct i2c_msg {
+	uint16_t addr;
+	uint16_t flags;
+	uint16_t len;
+	uint8_t *buf;
+};
+
+static void bcm_kona_i2c_send_cmd_to_ctrl(struct bcm_kona_i2c_dev *dev,
+					  enum bcm_kona_cmd_t cmd)
+{
+	debug("%s, %d\n", __func__, cmd);
+
+	switch (cmd) {
+	case BCM_CMD_NOACTION:
+		writel((CS_CMD_CMD_NO_ACTION << CS_CMD_SHIFT) |
+		       (CS_EN_CMD_ENABLE_BSC << CS_EN_SHIFT),
+		       dev->base + CS_OFFSET);
+		break;
+
+	case BCM_CMD_START:
+		writel((CS_ACK_CMD_GEN_START << CS_ACK_SHIFT) |
+		       (CS_CMD_CMD_START_RESTART << CS_CMD_SHIFT) |
+		       (CS_EN_CMD_ENABLE_BSC << CS_EN_SHIFT),
+		       dev->base + CS_OFFSET);
+		break;
+
+	case BCM_CMD_RESTART:
+		writel((CS_ACK_CMD_GEN_RESTART << CS_ACK_SHIFT) |
+		       (CS_CMD_CMD_START_RESTART << CS_CMD_SHIFT) |
+		       (CS_EN_CMD_ENABLE_BSC << CS_EN_SHIFT),
+		       dev->base + CS_OFFSET);
+		break;
+
+	case BCM_CMD_STOP:
+		writel((CS_CMD_CMD_STOP << CS_CMD_SHIFT) |
+		       (CS_EN_CMD_ENABLE_BSC << CS_EN_SHIFT),
+		       dev->base + CS_OFFSET);
+		break;
+
+	default:
+		printf("Unknown command %d\n", cmd);
+	}
+}
+
+static void bcm_kona_i2c_enable_clock(struct bcm_kona_i2c_dev *dev)
+{
+	writel(readl(dev->base + CLKEN_OFFSET) | CLKEN_CLKEN_MASK,
+	       dev->base + CLKEN_OFFSET);
+}
+
+static void bcm_kona_i2c_disable_clock(struct bcm_kona_i2c_dev *dev)
+{
+	writel(readl(dev->base + CLKEN_OFFSET) & ~CLKEN_CLKEN_MASK,
+	       dev->base + CLKEN_OFFSET);
+}
+
+/* Wait until at least one of the mask bit(s) are set */
+static unsigned long wait_for_int_timeout(struct bcm_kona_i2c_dev *dev,
+					  unsigned long time_left,
+					  uint32_t mask)
+{
+	uint32_t status;
+
+	while (time_left) {
+		status = readl(dev->base + ISR_OFFSET);
+
+		if ((status & ~ISR_RESERVED_MASK) == 0) {
+			debug("Bogus I2C interrupt 0x%x\n", status);
+			continue;
+		}
+
+		/* Must flush the TX FIFO when NAK detected */
+		if (status & ISR_NOACK_MASK)
+			writel(TXFCR_FIFO_FLUSH_MASK | TXFCR_FIFO_EN_MASK,
+			       dev->base + TXFCR_OFFSET);
+
+		writel(status & ~ISR_RESERVED_MASK, dev->base + ISR_OFFSET);
+
+		if (status & mask) {
+			/* We are done since one of the mask bits are set */
+			return time_left;
+		}
+		udelay(WAIT_INT_CHK);
+		time_left -= WAIT_INT_CHK;
+	}
+	return 0;
+}
+
+/* Send command to I2C bus */
+static int bcm_kona_send_i2c_cmd(struct bcm_kona_i2c_dev *dev,
+				 enum bcm_kona_cmd_t cmd)
+{
+	int rc = 0;
+	unsigned long time_left = I2C_TIMEOUT;
+
+	/* Send the command */
+	bcm_kona_i2c_send_cmd_to_ctrl(dev, cmd);
+
+	/* Wait for transaction to finish or timeout */
+	time_left = wait_for_int_timeout(dev, time_left, IER_I2C_INT_EN_MASK);
+
+	if (!time_left) {
+		printf("controller timed out\n");
+		rc = -ETIMEDOUT;
+	}
+
+	/* Clear command */
+	bcm_kona_i2c_send_cmd_to_ctrl(dev, BCM_CMD_NOACTION);
+
+	return rc;
+}
+
+/* Read a single RX FIFO worth of data from the i2c bus */
+static int bcm_kona_i2c_read_fifo_single(struct bcm_kona_i2c_dev *dev,
+					 uint8_t *buf, unsigned int len,
+					 unsigned int last_byte_nak)
+{
+	unsigned long time_left = I2C_TIMEOUT;
+
+	/* Start the RX FIFO */
+	writel((last_byte_nak << RXFCR_NACK_EN_SHIFT) |
+	       (len << RXFCR_READ_COUNT_SHIFT), dev->base + RXFCR_OFFSET);
+
+	/* Wait for FIFO read to complete */
+	time_left =
+	    wait_for_int_timeout(dev, time_left, IER_READ_COMPLETE_INT_MASK);
+
+	if (!time_left) {
+		printf("RX FIFO time out\n");
+		return -EREMOTEIO;
+	}
+
+	/* Read data from FIFO */
+	for (; len > 0; len--, buf++)
+		*buf = readl(dev->base + RXFIFORDOUT_OFFSET);
+
+	return 0;
+}
+
+/* Read any amount of data using the RX FIFO from the i2c bus */
+static int bcm_kona_i2c_read_fifo(struct bcm_kona_i2c_dev *dev,
+				  struct i2c_msg *msg)
+{
+	unsigned int bytes_to_read = MAX_RX_FIFO_SIZE;
+	unsigned int last_byte_nak = 0;
+	unsigned int bytes_read = 0;
+	int rc;
+
+	uint8_t *tmp_buf = msg->buf;
+
+	while (bytes_read < msg->len) {
+		if (msg->len - bytes_read <= MAX_RX_FIFO_SIZE) {
+			last_byte_nak = 1;	/* NAK last byte of transfer */
+			bytes_to_read = msg->len - bytes_read;
+		}
+
+		rc = bcm_kona_i2c_read_fifo_single(dev, tmp_buf, bytes_to_read,
+						   last_byte_nak);
+		if (rc < 0)
+			return -EREMOTEIO;
+
+		bytes_read += bytes_to_read;
+		tmp_buf += bytes_to_read;
+	}
+
+	return 0;
+}
+
+/* Write a single byte of data to the i2c bus */
+static int bcm_kona_i2c_write_byte(struct bcm_kona_i2c_dev *dev, uint8_t data,
+				   unsigned int nak_expected)
+{
+	unsigned long time_left = I2C_TIMEOUT;
+	unsigned int nak_received;
+
+	/* Clear pending session done interrupt */
+	writel(ISR_SES_DONE_MASK, dev->base + ISR_OFFSET);
+
+	/* Send one byte of data */
+	writel(data, dev->base + DAT_OFFSET);
+
+	time_left = wait_for_int_timeout(dev, time_left, IER_I2C_INT_EN_MASK);
+
+	if (!time_left) {
+		debug("controller timed out\n");
+		return -ETIMEDOUT;
+	}
+
+	nak_received = readl(dev->base + CS_OFFSET) & CS_ACK_MASK ? 1 : 0;
+
+	if (nak_received ^ nak_expected) {
+		debug("unexpected NAK/ACK\n");
+		return -EREMOTEIO;
+	}
+
+	return 0;
+}
+
+/* Write a single TX FIFO worth of data to the i2c bus */
+static int bcm_kona_i2c_write_fifo_single(struct bcm_kona_i2c_dev *dev,
+					  uint8_t *buf, unsigned int len)
+{
+	int k;
+	unsigned long time_left = I2C_TIMEOUT;
+	unsigned int fifo_status;
+
+	/* Write data into FIFO */
+	for (k = 0; k < len; k++)
+		writel(buf[k], (dev->base + DAT_OFFSET));
+
+	/* Wait for FIFO to empty */
+	do {
+		time_left =
+		    wait_for_int_timeout(dev, time_left,
+					 (IER_FIFO_INT_EN_MASK |
+					  IER_NOACK_EN_MASK));
+		fifo_status = readl(dev->base + FIFO_STATUS_OFFSET);
+	} while (time_left && !(fifo_status & FIFO_STATUS_TXFIFO_EMPTY_MASK));
+
+	/* Check if there was a NAK */
+	if (readl(dev->base + CS_OFFSET) & CS_ACK_MASK) {
+		printf("unexpected NAK\n");
+		return -EREMOTEIO;
+	}
+
+	/* Check if a timeout occured */
+	if (!time_left) {
+		printf("completion timed out\n");
+		return -EREMOTEIO;
+	}
+
+	return 0;
+}
+
+/* Write any amount of data using TX FIFO to the i2c bus */
+static int bcm_kona_i2c_write_fifo(struct bcm_kona_i2c_dev *dev,
+				   struct i2c_msg *msg)
+{
+	unsigned int bytes_to_write = MAX_TX_FIFO_SIZE;
+	unsigned int bytes_written = 0;
+	int rc;
+
+	uint8_t *tmp_buf = msg->buf;
+
+	while (bytes_written < msg->len) {
+		if (msg->len - bytes_written <= MAX_TX_FIFO_SIZE)
+			bytes_to_write = msg->len - bytes_written;
+
+		rc = bcm_kona_i2c_write_fifo_single(dev, tmp_buf,
+						    bytes_to_write);
+		if (rc < 0)
+			return -EREMOTEIO;
+
+		bytes_written += bytes_to_write;
+		tmp_buf += bytes_to_write;
+	}
+
+	return 0;
+}
+
+/* Send i2c address */
+static int bcm_kona_i2c_do_addr(struct bcm_kona_i2c_dev *dev,
+				struct i2c_msg *msg)
+{
+	unsigned char addr;
+
+	if (msg->flags & I2C_M_TEN) {
+		/* First byte is 11110XX0 where XX is upper 2 bits */
+		addr = 0xf0 | ((msg->addr & 0x300) >> 7);
+		if (bcm_kona_i2c_write_byte(dev, addr, 0) < 0)
+			return -EREMOTEIO;
+
+		/* Second byte is the remaining 8 bits */
+		addr = msg->addr & 0xff;
+		if (bcm_kona_i2c_write_byte(dev, addr, 0) < 0)
+			return -EREMOTEIO;
+
+		if (msg->flags & I2C_M_RD) {
+			/* For read, send restart command */
+			if (bcm_kona_send_i2c_cmd(dev, BCM_CMD_RESTART) < 0)
+				return -EREMOTEIO;
+
+			/* Then re-send the first byte with the read bit set */
+			addr = 0xf0 | ((msg->addr & 0x300) >> 7) | 0x01;
+			if (bcm_kona_i2c_write_byte(dev, addr, 0) < 0)
+				return -EREMOTEIO;
+		}
+	} else {
+		addr = msg->addr << 1;
+
+		if (msg->flags & I2C_M_RD)
+			addr |= 1;
+
+		if (bcm_kona_i2c_write_byte(dev, addr, 0) < 0)
+			return -EREMOTEIO;
+	}
+
+	return 0;
+}
+
+static void bcm_kona_i2c_enable_autosense(struct bcm_kona_i2c_dev *dev)
+{
+	writel(readl(dev->base + CLKEN_OFFSET) & ~CLKEN_AUTOSENSE_OFF_MASK,
+	       dev->base + CLKEN_OFFSET);
+}
+
+static void bcm_kona_i2c_config_timing(struct bcm_kona_i2c_dev *dev)
+{
+	writel(readl(dev->base + HSTIM_OFFSET) & ~HSTIM_HS_MODE_MASK,
+	       dev->base + HSTIM_OFFSET);
+
+	writel((dev->std_cfg->prescale << TIM_PRESCALE_SHIFT) |
+	       (dev->std_cfg->time_p << TIM_P_SHIFT) |
+	       (dev->std_cfg->no_div << TIM_NO_DIV_SHIFT) |
+	       (dev->std_cfg->time_div << TIM_DIV_SHIFT),
+	       dev->base + TIM_OFFSET);
+
+	writel((dev->std_cfg->time_m << CLKEN_M_SHIFT) |
+	       (dev->std_cfg->time_n << CLKEN_N_SHIFT) |
+	       CLKEN_CLKEN_MASK, dev->base + CLKEN_OFFSET);
+}
+
+/* Master transfer function */
+static int bcm_kona_i2c_xfer(struct bcm_kona_i2c_dev *dev,
+			     struct i2c_msg msgs[], int num)
+{
+	struct i2c_msg *pmsg;
+	int rc = 0;
+	int i;
+
+	/* Enable pad output */
+	writel(0, dev->base + PADCTL_OFFSET);
+
+	/* Enable internal clocks */
+	bcm_kona_i2c_enable_clock(dev);
+
+	/* Send start command */
+	rc = bcm_kona_send_i2c_cmd(dev, BCM_CMD_START);
+	if (rc < 0) {
+		printf("Start command failed rc = %d\n", rc);
+		goto xfer_disable_pad;
+	}
+
+	/* Loop through all messages */
+	for (i = 0; i < num; i++) {
+		pmsg = &msgs[i];
+
+		/* Send restart for subsequent messages */
+		if ((i != 0) && ((pmsg->flags & I2C_M_NOSTART) == 0)) {
+			rc = bcm_kona_send_i2c_cmd(dev, BCM_CMD_RESTART);
+			if (rc < 0) {
+				printf("restart cmd failed rc = %d\n", rc);
+				goto xfer_send_stop;
+			}
+		}
+
+		/* Send slave address */
+		if (!(pmsg->flags & I2C_M_NOSTART)) {
+			rc = bcm_kona_i2c_do_addr(dev, pmsg);
+			if (rc < 0) {
+				debug("NAK from addr %2.2x msg#%d rc = %d\n",
+				      pmsg->addr, i, rc);
+				goto xfer_send_stop;
+			}
+		}
+
+		/* Perform data transfer */
+		if (pmsg->flags & I2C_M_RD) {
+			rc = bcm_kona_i2c_read_fifo(dev, pmsg);
+			if (rc < 0) {
+				printf("read failure\n");
+				goto xfer_send_stop;
+			}
+		} else {
+			rc = bcm_kona_i2c_write_fifo(dev, pmsg);
+			if (rc < 0) {
+				printf("write failure");
+				goto xfer_send_stop;
+			}
+		}
+	}
+
+	rc = num;
+
+xfer_send_stop:
+	/* Send a STOP command */
+	bcm_kona_send_i2c_cmd(dev, BCM_CMD_STOP);
+
+xfer_disable_pad:
+	/* Disable pad output */
+	writel(PADCTL_PAD_OUT_EN_MASK, dev->base + PADCTL_OFFSET);
+
+	/* Stop internal clock */
+	bcm_kona_i2c_disable_clock(dev);
+
+	return rc;
+}
+
+static uint bcm_kona_i2c_assign_bus_speed(struct bcm_kona_i2c_dev *dev,
+					  uint speed)
+{
+	switch (speed) {
+	case 100000:
+		dev->std_cfg = &std_cfg_table[BCM_SPD_100K];
+		break;
+	case 400000:
+		dev->std_cfg = &std_cfg_table[BCM_SPD_400K];
+		break;
+	case 1000000:
+		dev->std_cfg = &std_cfg_table[BCM_SPD_1MHZ];
+		break;
+	default:
+		printf("%d hz bus speed not supported\n", speed);
+		return -EINVAL;
+	}
+	dev->speed = speed;
+	return 0;
+}
+
+static void bcm_kona_i2c_init(struct bcm_kona_i2c_dev *dev)
+{
+	/* Parse bus speed */
+	bcm_kona_i2c_assign_bus_speed(dev, dev->speed);
+
+	/* Enable internal clocks */
+	bcm_kona_i2c_enable_clock(dev);
+
+	/* Configure internal dividers */
+	bcm_kona_i2c_config_timing(dev);
+
+	/* Disable timeout */
+	writel(0, dev->base + TOUT_OFFSET);
+
+	/* Enable autosense */
+	bcm_kona_i2c_enable_autosense(dev);
+
+	/* Enable TX FIFO */
+	writel(TXFCR_FIFO_FLUSH_MASK | TXFCR_FIFO_EN_MASK,
+	       dev->base + TXFCR_OFFSET);
+
+	/* Mask all interrupts */
+	writel(0, dev->base + IER_OFFSET);
+
+	/* Clear all pending interrupts */
+	writel(ISR_CMDBUSY_MASK |
+	       ISR_READ_COMPLETE_MASK |
+	       ISR_SES_DONE_MASK |
+	       ISR_ERR_MASK |
+	       ISR_TXFIFOEMPTY_MASK | ISR_NOACK_MASK, dev->base + ISR_OFFSET);
+
+	/* Enable the controller but leave it idle */
+	bcm_kona_i2c_send_cmd_to_ctrl(dev, BCM_CMD_NOACTION);
+
+	/* Disable pad output */
+	writel(PADCTL_PAD_OUT_EN_MASK, dev->base + PADCTL_OFFSET);
+}
+
+/*
+ * uboot layer
+ */
+struct bcm_kona_i2c_dev *kona_get_dev(struct i2c_adapter *adap)
+{
+	return &g_i2c_devs[adap->hwadapnr];
+}
+
+static void kona_i2c_init(struct i2c_adapter *adap, int speed, int slaveaddr)
+{
+	struct bcm_kona_i2c_dev *dev = kona_get_dev(adap);
+
+	if (clk_bsc_enable(dev->base))
+		return;
+
+	bcm_kona_i2c_init(dev);
+}
+
+static int kona_i2c_read(struct i2c_adapter *adap, uchar chip, uint addr,
+			 int alen, uchar *buffer, int len)
+{
+	/* msg[0] writes the addr, msg[1] reads the data */
+	struct i2c_msg msg[2];
+	unsigned char msgbuf0[64];
+	struct bcm_kona_i2c_dev *dev = kona_get_dev(adap);
+
+	msg[0].addr = chip;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = msgbuf0;	/* msgbuf0 contains incrementing reg addr */
+
+	msg[1].addr = chip;
+	msg[1].flags = I2C_M_RD;
+	/* msg[1].buf dest ptr increments each read */
+
+	msgbuf0[0] = (unsigned char)addr;
+	msg[1].buf = buffer;
+	msg[1].len = len;
+	if (bcm_kona_i2c_xfer(dev, msg, 2) < 0) {
+		/* Sending 2 i2c messages */
+		kona_i2c_init(adap, adap->speed, adap->slaveaddr);
+		debug("I2C read: I/O error\n");
+		return -EIO;
+	}
+	return 0;
+}
+
+static int kona_i2c_write(struct i2c_adapter *adap, uchar chip, uint addr,
+			  int alen, uchar *buffer, int len)
+{
+	struct i2c_msg msg[0];
+	unsigned char msgbuf0[64];
+	unsigned int i;
+	struct bcm_kona_i2c_dev *dev = kona_get_dev(adap);
+
+	msg[0].addr = chip;
+	msg[0].flags = 0;
+	msg[0].len = 2;		/* addr byte plus data */
+	msg[0].buf = msgbuf0;
+
+	for (i = 0; i < len; i++) {
+		msgbuf0[0] = addr++;
+		msgbuf0[1] = buffer[i];
+		if (bcm_kona_i2c_xfer(dev, msg, 1) < 0) {
+			kona_i2c_init(adap, adap->speed, adap->slaveaddr);
+			debug("I2C write: I/O error\n");
+			return -EIO;
+		}
+	}
+	return 0;
+}
+
+static int kona_i2c_probe(struct i2c_adapter *adap, uchar chip)
+{
+	uchar tmp;
+
+	/*
+	 * read addr 0x0 of the given chip.
+	 */
+	return kona_i2c_read(adap, chip, 0x0, 1, &tmp, 1);
+}
+
+static uint kona_i2c_set_bus_speed(struct i2c_adapter *adap, uint speed)
+{
+	struct bcm_kona_i2c_dev *dev = kona_get_dev(adap);
+	return bcm_kona_i2c_assign_bus_speed(dev, speed);
+}
+
+/*
+ * Register kona i2c adapters. Keep the order below so
+ * that the bus number matches the adapter number.
+ */
+#define DEF_ADAPTER(num) \
+U_BOOT_I2C_ADAP_COMPLETE(kona##num, kona_i2c_init, kona_i2c_probe, \
+			 kona_i2c_read, kona_i2c_write, \
+			 kona_i2c_set_bus_speed, DEF_SPD, 0x00, num)
+
+#ifdef CONFIG_SYS_I2C_BASE0
+	DEF_ADAPTER(0)
+#endif
+#ifdef CONFIG_SYS_I2C_BASE1
+	DEF_ADAPTER(1)
+#endif
+#ifdef CONFIG_SYS_I2C_BASE2
+	DEF_ADAPTER(2)
+#endif
+#ifdef CONFIG_SYS_I2C_BASE3
+	DEF_ADAPTER(3)
+#endif
+#ifdef CONFIG_SYS_I2C_BASE4
+	DEF_ADAPTER(4)
+#endif
+#ifdef CONFIG_SYS_I2C_BASE5
+	DEF_ADAPTER(5)
+#endif
diff --git a/drivers/i2c/mxs_i2c.c b/drivers/i2c/mxs_i2c.c
index a298c95..de3b194 100644
--- a/drivers/i2c/mxs_i2c.c
+++ b/drivers/i2c/mxs_i2c.c
@@ -64,16 +64,17 @@ static void mxs_i2c_setup_read(uint8_t chip, int len)
 	writel(I2C_QUEUECTRL_QUEUE_RUN, &i2c_regs->hw_i2c_queuectrl_set);
 }
 
-static void mxs_i2c_write(uchar chip, uint addr, int alen,
+static int mxs_i2c_write(uchar chip, uint addr, int alen,
 			uchar *buf, int blen, int stop)
 {
 	struct mxs_i2c_regs *i2c_regs = (struct mxs_i2c_regs *)MXS_I2C0_BASE;
-	uint32_t data;
+	uint32_t data, tmp;
 	int i, remain, off;
+	int timeout = MXS_I2C_MAX_TIMEOUT;
 
 	if ((alen > 4) || (alen == 0)) {
 		debug("MXS I2C: Invalid address length\n");
-		return;
+		return -EINVAL;
 	}
 
 	if (stop)
@@ -106,6 +107,19 @@ static void mxs_i2c_write(uchar chip, uint addr, int alen,
 		writel(data >> remain, &i2c_regs->hw_i2c_data);
 
 	writel(I2C_QUEUECTRL_QUEUE_RUN, &i2c_regs->hw_i2c_queuectrl_set);
+
+	while (--timeout) {
+		tmp = readl(&i2c_regs->hw_i2c_queuestat);
+		if (tmp & I2C_QUEUESTAT_WR_QUEUE_EMPTY)
+			break;
+	}
+
+	if (!timeout) {
+		debug("MXS I2C: Failed transmitting data!\n");
+		return -EINVAL;
+	}
+
+	return 0;
 }
 
 static int mxs_i2c_wait_for_ack(void)
@@ -154,7 +168,12 @@ int i2c_read(uchar chip, uint addr, int alen, uchar *buffer, int len)
 	int ret;
 	int i;
 
-	mxs_i2c_write(chip, addr, alen, NULL, 0, 0);
+	ret = mxs_i2c_write(chip, addr, alen, NULL, 0, 0);
+	if (ret) {
+		debug("MXS I2C: Failed writing address\n");
+		return ret;
+	}
+
 	ret = mxs_i2c_wait_for_ack();
 	if (ret) {
 		debug("MXS I2C: Failed writing address\n");
@@ -193,7 +212,12 @@ int i2c_read(uchar chip, uint addr, int alen, uchar *buffer, int len)
 int i2c_write(uchar chip, uint addr, int alen, uchar *buffer, int len)
 {
 	int ret;
-	mxs_i2c_write(chip, addr, alen, buffer, len, 1);
+	ret = mxs_i2c_write(chip, addr, alen, buffer, len, 1);
+	if (ret) {
+		debug("MXS I2C: Failed writing address\n");
+		return ret;
+	}
+
 	ret = mxs_i2c_wait_for_ack();
 	if (ret)
 		debug("MXS I2C: Failed writing address\n");
@@ -204,8 +228,9 @@ int i2c_write(uchar chip, uint addr, int alen, uchar *buffer, int len)
 int i2c_probe(uchar chip)
 {
 	int ret;
-	mxs_i2c_write(chip, 0, 1, NULL, 0, 1);
-	ret = mxs_i2c_wait_for_ack();
+	ret = mxs_i2c_write(chip, 0, 1, NULL, 0, 1);
+	if (!ret)
+		ret = mxs_i2c_wait_for_ack();
 	mxs_i2c_reset();
 	return ret;
 }
diff --git a/drivers/i2c/omap24xx_i2c.c b/drivers/i2c/omap24xx_i2c.c
index c784004..a39b591 100644
--- a/drivers/i2c/omap24xx_i2c.c
+++ b/drivers/i2c/omap24xx_i2c.c
@@ -32,6 +32,10 @@
  * - Status functions now read irqstatus_raw as per TRM guidelines
  *   (except for OMAP243X and OMAP34XX).
  * - Driver now supports up to I2C5 (OMAP5).
+ *
+ * Copyright (c) 2014 Hannes Petermaier <oe5hpm@oevsv.at>, B&R
+ * - Added support for set_speed
+ *
  */
 
 #include <common.h>
@@ -53,43 +57,66 @@ static int wait_for_bb(struct i2c_adapter *adap);
 static struct i2c *omap24_get_base(struct i2c_adapter *adap);
 static u16 wait_for_event(struct i2c_adapter *adap);
 static void flush_fifo(struct i2c_adapter *adap);
-
-static void omap24_i2c_init(struct i2c_adapter *adap, int speed, int slaveadd)
+static int omap24_i2c_findpsc(u32 *pscl, u32 *psch, uint speed)
 {
-	struct i2c *i2c_base = omap24_get_base(adap);
-	int psc, fsscll, fssclh;
-	int hsscll = 0, hssclh = 0;
-	u32 scll, sclh;
-	int timeout = I2C_TIMEOUT;
+	unsigned int sampleclk, prescaler;
+	int fsscll, fssclh;
 
-	/* Only handle standard, fast and high speeds */
-	if ((speed != OMAP_I2C_STANDARD) &&
-	    (speed != OMAP_I2C_FAST_MODE) &&
-	    (speed != OMAP_I2C_HIGH_SPEED)) {
-		printf("Error : I2C unsupported speed %d\n", speed);
-		return;
-	}
+	speed <<= 1;
+	prescaler = 0;
+	/*
+	 * some divisors may cause a precission loss, but shouldn't
+	 * be a big thing, because i2c_clk is then allready very slow.
+	 */
+	while (prescaler <= 0xFF) {
+		sampleclk = I2C_IP_CLK / (prescaler+1);
 
-	psc = I2C_IP_CLK / I2C_INTERNAL_SAMPLING_CLK;
-	psc -= 1;
-	if (psc < I2C_PSC_MIN) {
-		printf("Error : I2C unsupported prescalar %d\n", psc);
-		return;
+		fsscll = sampleclk / speed;
+		fssclh = fsscll;
+		fsscll -= I2C_FASTSPEED_SCLL_TRIM;
+		fssclh -= I2C_FASTSPEED_SCLH_TRIM;
+
+		if (((fsscll > 0) && (fssclh > 0)) &&
+		    ((fsscll <= (255-I2C_FASTSPEED_SCLL_TRIM)) &&
+		    (fssclh <= (255-I2C_FASTSPEED_SCLH_TRIM)))) {
+			if (pscl)
+				*pscl = fsscll;
+			if (psch)
+				*psch = fssclh;
+
+			return prescaler;
+		}
+		prescaler++;
 	}
+	return -1;
+}
+static uint omap24_i2c_setspeed(struct i2c_adapter *adap, uint speed)
+{
+	struct i2c *i2c_base = omap24_get_base(adap);
+	int psc, fsscll = 0, fssclh = 0;
+	int hsscll = 0, hssclh = 0;
+	u32 scll = 0, sclh = 0;
 
-	if (speed == OMAP_I2C_HIGH_SPEED) {
+	if (speed >= OMAP_I2C_HIGH_SPEED) {
 		/* High speed */
+		psc = I2C_IP_CLK / I2C_INTERNAL_SAMPLING_CLK;
+		psc -= 1;
+		if (psc < I2C_PSC_MIN) {
+			printf("Error : I2C unsupported prescaler %d\n", psc);
+			return -1;
+		}
 
 		/* For first phase of HS mode */
-		fsscll = fssclh = I2C_INTERNAL_SAMPLING_CLK /
-			(2 * OMAP_I2C_FAST_MODE);
+		fsscll = I2C_INTERNAL_SAMPLING_CLK / (2 * speed);
+
+		fssclh = fsscll;
 
 		fsscll -= I2C_HIGHSPEED_PHASE_ONE_SCLL_TRIM;
 		fssclh -= I2C_HIGHSPEED_PHASE_ONE_SCLH_TRIM;
 		if (((fsscll < 0) || (fssclh < 0)) ||
 		    ((fsscll > 255) || (fssclh > 255))) {
 			puts("Error : I2C initializing first phase clock\n");
-			return;
+			return -1;
 		}
 
 		/* For second phase of HS mode */
@@ -100,7 +127,7 @@ static void omap24_i2c_init(struct i2c_adapter *adap, int speed, int slaveadd)
 		if (((fsscll < 0) || (fssclh < 0)) ||
 		    ((fsscll > 255) || (fssclh > 255))) {
 			puts("Error : I2C initializing second phase clock\n");
-			return;
+			return -1;
 		}
 
 		scll = (unsigned int)hsscll << 8 | (unsigned int)fsscll;
@@ -108,20 +135,29 @@ static void omap24_i2c_init(struct i2c_adapter *adap, int speed, int slaveadd)
 
 	} else {
 		/* Standard and fast speed */
-		fsscll = fssclh = I2C_INTERNAL_SAMPLING_CLK / (2 * speed);
-
-		fsscll -= I2C_FASTSPEED_SCLL_TRIM;
-		fssclh -= I2C_FASTSPEED_SCLH_TRIM;
-		if (((fsscll < 0) || (fssclh < 0)) ||
-		    ((fsscll > 255) || (fssclh > 255))) {
+		psc = omap24_i2c_findpsc(&scll, &sclh, speed);
+		if (0 > psc) {
 			puts("Error : I2C initializing clock\n");
-			return;
+			return -1;
 		}
-
-		scll = (unsigned int)fsscll;
-		sclh = (unsigned int)fssclh;
 	}
 
+	adap->speed	= speed;
+	adap->waitdelay = (10000000 / speed) * 2; /* wait for 20 clkperiods */
+	writew(0, &i2c_base->con);
+	writew(psc, &i2c_base->psc);
+	writew(scll, &i2c_base->scll);
+	writew(sclh, &i2c_base->sclh);
+	writew(I2C_CON_EN, &i2c_base->con);
+	writew(0xFFFF, &i2c_base->stat);	/* clear all pending status */
+
+	return 0;
+}
+static void omap24_i2c_init(struct i2c_adapter *adap, int speed, int slaveadd)
+{
+	struct i2c *i2c_base = omap24_get_base(adap);
+	int timeout = I2C_TIMEOUT;
+
 	if (readw(&i2c_base->con) & I2C_CON_EN) {
 		writew(0, &i2c_base->con);
 		udelay(50000);
@@ -139,14 +175,14 @@ static void omap24_i2c_init(struct i2c_adapter *adap, int speed, int slaveadd)
 		udelay(1000);
 	}
 
-	writew(0, &i2c_base->con);
-	writew(psc, &i2c_base->psc);
-	writew(scll, &i2c_base->scll);
-	writew(sclh, &i2c_base->sclh);
+	if (0 != omap24_i2c_setspeed(adap, speed)) {
+		printf("ERROR: failed to setup I2C bus-speed!\n");
+		return;
+	}
 
 	/* own address */
 	writew(slaveadd, &i2c_base->oa);
-	writew(I2C_CON_EN, &i2c_base->con);
+
 #if defined(CONFIG_OMAP243X) || defined(CONFIG_OMAP34XX)
 	/*
 	 * Have to enable interrupts for OMAP2/3, these IPs don't have
@@ -165,7 +201,8 @@ static void flush_fifo(struct i2c_adapter *adap)
 	struct i2c *i2c_base = omap24_get_base(adap);
 	u16 stat;
 
-	/* note: if you try and read data when its not there or ready
+	/*
+	 * note: if you try and read data when its not there or ready
 	 * you get a bus error
 	 */
 	while (1) {
@@ -220,8 +257,8 @@ static int omap24_i2c_probe(struct i2c_adapter *adap, uchar chip)
 
 	/* Check for ACK (!NAK) */
 	if (!(status & I2C_STAT_NACK)) {
-		res = 0;			/* Device found */
-		udelay(I2C_WAIT);		/* Required by AM335X in SPL */
+		res = 0;				/* Device found */
+		udelay(adap->waitdelay);/* Required by AM335X in SPL */
 		/* Abort transfer (force idle state) */
 		writew(I2C_CON_MST | I2C_CON_TRX, &i2c_base->con); /* Reset */
 		udelay(1000);
@@ -307,7 +344,7 @@ static int omap24_i2c_read(struct i2c_adapter *adap, uchar chip, uint addr,
 				       adap->hwadapnr, status);
 				goto rd_exit;
 			}
-			if (status == 0 || status & I2C_STAT_NACK) {
+			if (status == 0 || (status & I2C_STAT_NACK)) {
 				i2c_error = 1;
 				printf("i2c_read: error waiting for addr ACK (status=0x%x)\n",
 				       status);
@@ -351,7 +388,7 @@ static int omap24_i2c_read(struct i2c_adapter *adap, uchar chip, uint addr,
 			       adap->hwadapnr, status);
 			goto rd_exit;
 		}
-		if (status == 0 || status & I2C_STAT_NACK) {
+		if (status == 0 || (status & I2C_STAT_NACK)) {
 			i2c_error = 1;
 			goto rd_exit;
 		}
@@ -379,6 +416,7 @@ static int omap24_i2c_write(struct i2c_adapter *adap, uchar chip, uint addr,
 	int i;
 	u16 status;
 	int i2c_error = 0;
+	int timeout = I2C_TIMEOUT;
 
 	if (alen < 0) {
 		puts("I2C write: addr len < 0\n");
@@ -428,7 +466,7 @@ static int omap24_i2c_write(struct i2c_adapter *adap, uchar chip, uint addr,
 			       adap->hwadapnr, status);
 			goto wr_exit;
 		}
-		if (status == 0 || status & I2C_STAT_NACK) {
+		if (status == 0 || (status & I2C_STAT_NACK)) {
 			i2c_error = 1;
 			printf("i2c_write: error waiting for addr ACK (status=0x%x)\n",
 			       status);
@@ -448,7 +486,7 @@ static int omap24_i2c_write(struct i2c_adapter *adap, uchar chip, uint addr,
 	/* Address phase is over, now write data */
 	for (i = 0; i < len; i++) {
 		status = wait_for_event(adap);
-		if (status == 0 || status & I2C_STAT_NACK) {
+		if (status == 0 || (status & I2C_STAT_NACK)) {
 			i2c_error = 1;
 			printf("i2c_write: error waiting for data ACK (status=0x%x)\n",
 			       status);
@@ -464,6 +502,15 @@ static int omap24_i2c_write(struct i2c_adapter *adap, uchar chip, uint addr,
 			goto wr_exit;
 		}
 	}
+	/*
+	 * poll ARDY bit for making sure that last byte really has been
+	 * transferred on the bus.
+	 */
+	do {
+		status = wait_for_event(adap);
+	} while (!(status & I2C_STAT_ARDY) && timeout--);
+	if (timeout <= 0)
+		printf("i2c_write: timed out writig last byte!\n");
 
 wr_exit:
 	flush_fifo(adap);
@@ -490,7 +537,7 @@ static int wait_for_bb(struct i2c_adapter *adap)
 		I2C_STAT_BB) && timeout--) {
 #endif
 		writew(stat, &i2c_base->stat);
-		udelay(I2C_WAIT);
+		udelay(adap->waitdelay);
 	}
 
 	if (timeout <= 0) {
@@ -513,7 +560,7 @@ static u16 wait_for_event(struct i2c_adapter *adap)
 	int timeout = I2C_TIMEOUT;
 
 	do {
-		udelay(I2C_WAIT);
+		udelay(adap->waitdelay);
 #if defined(CONFIG_OMAP243X) || defined(CONFIG_OMAP34XX)
 		status = readw(&i2c_base->stat);
 #else
@@ -580,12 +627,12 @@ static struct i2c *omap24_get_base(struct i2c_adapter *adap)
 #endif
 
 U_BOOT_I2C_ADAP_COMPLETE(omap24_0, omap24_i2c_init, omap24_i2c_probe,
-			 omap24_i2c_read, omap24_i2c_write, NULL,
+			 omap24_i2c_read, omap24_i2c_write, omap24_i2c_setspeed,
 			 CONFIG_SYS_OMAP24_I2C_SPEED,
 			 CONFIG_SYS_OMAP24_I2C_SLAVE,
 			 0)
 U_BOOT_I2C_ADAP_COMPLETE(omap24_1, omap24_i2c_init, omap24_i2c_probe,
-			 omap24_i2c_read, omap24_i2c_write, NULL,
+			 omap24_i2c_read, omap24_i2c_write, omap24_i2c_setspeed,
 			 CONFIG_SYS_OMAP24_I2C_SPEED1,
 			 CONFIG_SYS_OMAP24_I2C_SLAVE1,
 			 1)
diff --git a/drivers/i2c/soft_i2c.c b/drivers/i2c/soft_i2c.c
index dfea54a..db9b402 100644
--- a/drivers/i2c/soft_i2c.c
+++ b/drivers/i2c/soft_i2c.c
@@ -29,9 +29,6 @@
 #include <asm/arch/gpio.h>
 #endif
 #endif
-#ifdef	CONFIG_IXP425			/* only valid for IXP425 */
-#include <asm/arch/ixp425.h>
-#endif
 #if defined(CONFIG_MPC852T) || defined(CONFIG_MPC866)
 #include <asm/io.h>
 #endif
diff --git a/drivers/i2c/zynq_i2c.c b/drivers/i2c/zynq_i2c.c
index 70a9aea..f1f6513 100644
--- a/drivers/i2c/zynq_i2c.c
+++ b/drivers/i2c/zynq_i2c.c
@@ -64,19 +64,21 @@ struct zynq_i2c_registers {
 #define ZYNQ_I2C_FIFO_DEPTH		16
 #define ZYNQ_I2C_TRANSFERT_SIZE_MAX	255 /* Controller transfer limit */
 
-#if defined(CONFIG_ZYNQ_I2C0)
-# define ZYNQ_I2C_BASE	ZYNQ_I2C_BASEADDR0
-#else
-# define ZYNQ_I2C_BASE	ZYNQ_I2C_BASEADDR1
-#endif
-
-static struct zynq_i2c_registers *zynq_i2c =
-	(struct zynq_i2c_registers *)ZYNQ_I2C_BASE;
+static struct zynq_i2c_registers *i2c_select(struct i2c_adapter *adap)
+{
+	return adap->hwadapnr ?
+		/* Zynq PS I2C1 */
+		(struct zynq_i2c_registers *)ZYNQ_I2C_BASEADDR1 :
+		/* Zynq PS I2C0 */
+		(struct zynq_i2c_registers *)ZYNQ_I2C_BASEADDR0;
+}
 
 /* I2C init called by cmd_i2c when doing 'i2c reset'. */
 static void zynq_i2c_init(struct i2c_adapter *adap, int requested_speed,
 			  int slaveadd)
 {
+	struct zynq_i2c_registers *zynq_i2c = i2c_select(adap);
+
 	/* 111MHz / ( (3 * 17) * 22 ) = ~100KHz */
 	writel((16 << ZYNQ_I2C_CONTROL_DIV_B_SHIFT) |
 		(2 << ZYNQ_I2C_CONTROL_DIV_A_SHIFT), &zynq_i2c->control);
@@ -87,7 +89,7 @@ static void zynq_i2c_init(struct i2c_adapter *adap, int requested_speed,
 }
 
 #ifdef DEBUG
-static void zynq_i2c_debug_status(void)
+static void zynq_i2c_debug_status(struct zynq_i2c_registers *zynq_i2c)
 {
 	int int_status;
 	int status;
@@ -129,7 +131,7 @@ static void zynq_i2c_debug_status(void)
 #endif
 
 /* Wait for an interrupt */
-static u32 zynq_i2c_wait(u32 mask)
+static u32 zynq_i2c_wait(struct zynq_i2c_registers *zynq_i2c, u32 mask)
 {
 	int timeout, int_status;
 
@@ -140,7 +142,7 @@ static u32 zynq_i2c_wait(u32 mask)
 			break;
 	}
 #ifdef DEBUG
-	zynq_i2c_debug_status();
+	zynq_i2c_debug_status(zynq_i2c));
 #endif
 	/* Clear interrupt status flags */
 	writel(int_status & mask, &zynq_i2c->interrupt_status);
@@ -154,6 +156,8 @@ static u32 zynq_i2c_wait(u32 mask)
  */
 static int zynq_i2c_probe(struct i2c_adapter *adap, u8 dev)
 {
+	struct zynq_i2c_registers *zynq_i2c = i2c_select(adap);
+
 	/* Attempt to read a byte */
 	setbits_le32(&zynq_i2c->control, ZYNQ_I2C_CONTROL_CLR_FIFO |
 		ZYNQ_I2C_CONTROL_RW);
@@ -162,7 +166,7 @@ static int zynq_i2c_probe(struct i2c_adapter *adap, u8 dev)
 	writel(dev, &zynq_i2c->address);
 	writel(1, &zynq_i2c->transfer_size);
 
-	return (zynq_i2c_wait(ZYNQ_I2C_INTERRUPT_COMP |
+	return (zynq_i2c_wait(zynq_i2c, ZYNQ_I2C_INTERRUPT_COMP |
 		ZYNQ_I2C_INTERRUPT_NACK) &
 		ZYNQ_I2C_INTERRUPT_COMP) ? 0 : -ETIMEDOUT;
 }
@@ -177,6 +181,7 @@ static int zynq_i2c_read(struct i2c_adapter *adap, u8 dev, uint addr,
 	u32 status;
 	u32 i = 0;
 	u8 *cur_data = data;
+	struct zynq_i2c_registers *zynq_i2c = i2c_select(adap);
 
 	/* Check the hardware can handle the requested bytes */
 	if ((length < 0) || (length > ZYNQ_I2C_TRANSFERT_SIZE_MAX))
@@ -189,20 +194,22 @@ static int zynq_i2c_read(struct i2c_adapter *adap, u8 dev, uint addr,
 	 * Temporarily disable restart (by clearing hold)
 	 * It doesn't seem to work.
 	 */
-	clrbits_le32(&zynq_i2c->control, ZYNQ_I2C_CONTROL_RW |
-		ZYNQ_I2C_CONTROL_HOLD);
+	clrbits_le32(&zynq_i2c->control, ZYNQ_I2C_CONTROL_HOLD);
 	writel(0xFF, &zynq_i2c->interrupt_status);
-	while (alen--)
-		writel(addr >> (8*alen), &zynq_i2c->data);
-	writel(dev, &zynq_i2c->address);
+	if (alen) {
+		clrbits_le32(&zynq_i2c->control, ZYNQ_I2C_CONTROL_RW);
+		writel(dev, &zynq_i2c->address);
+		while (alen--)
+			writel(addr >> (8 * alen), &zynq_i2c->data);
 
-	/* Wait for the address to be sent */
-	if (!zynq_i2c_wait(ZYNQ_I2C_INTERRUPT_COMP)) {
-		/* Release the bus */
-		clrbits_le32(&zynq_i2c->control, ZYNQ_I2C_CONTROL_HOLD);
-		return -ETIMEDOUT;
+		/* Wait for the address to be sent */
+		if (!zynq_i2c_wait(zynq_i2c, ZYNQ_I2C_INTERRUPT_COMP)) {
+			/* Release the bus */
+			clrbits_le32(&zynq_i2c->control, ZYNQ_I2C_CONTROL_HOLD);
+			return -ETIMEDOUT;
+		}
+		debug("Device acked address\n");
 	}
-	debug("Device acked address\n");
 
 	setbits_le32(&zynq_i2c->control, ZYNQ_I2C_CONTROL_CLR_FIFO |
 		ZYNQ_I2C_CONTROL_RW);
@@ -212,7 +219,7 @@ static int zynq_i2c_read(struct i2c_adapter *adap, u8 dev, uint addr,
 
 	/* Wait for data */
 	do {
-		status = zynq_i2c_wait(ZYNQ_I2C_INTERRUPT_COMP |
+		status = zynq_i2c_wait(zynq_i2c, ZYNQ_I2C_INTERRUPT_COMP |
 			ZYNQ_I2C_INTERRUPT_DATA);
 		if (!status) {
 			/* Release the bus */
@@ -241,27 +248,30 @@ static int zynq_i2c_write(struct i2c_adapter *adap, u8 dev, uint addr,
 			  int alen, u8 *data, int length)
 {
 	u8 *cur_data = data;
+	struct zynq_i2c_registers *zynq_i2c = i2c_select(adap);
 
 	/* Write the register address */
 	setbits_le32(&zynq_i2c->control, ZYNQ_I2C_CONTROL_CLR_FIFO |
 		ZYNQ_I2C_CONTROL_HOLD);
 	clrbits_le32(&zynq_i2c->control, ZYNQ_I2C_CONTROL_RW);
 	writel(0xFF, &zynq_i2c->interrupt_status);
-	while (alen--)
-		writel(addr >> (8*alen), &zynq_i2c->data);
-	/* Start the tranfer */
 	writel(dev, &zynq_i2c->address);
-	if (!zynq_i2c_wait(ZYNQ_I2C_INTERRUPT_COMP)) {
-		/* Release the bus */
-		clrbits_le32(&zynq_i2c->control, ZYNQ_I2C_CONTROL_HOLD);
-		return -ETIMEDOUT;
+	if (alen) {
+		while (alen--)
+			writel(addr >> (8 * alen), &zynq_i2c->data);
+		/* Start the tranfer */
+		if (!zynq_i2c_wait(zynq_i2c, ZYNQ_I2C_INTERRUPT_COMP)) {
+			/* Release the bus */
+			clrbits_le32(&zynq_i2c->control, ZYNQ_I2C_CONTROL_HOLD);
+			return -ETIMEDOUT;
+		}
+		debug("Device acked address\n");
 	}
 
-	debug("Device acked address\n");
 	while (length--) {
 		writel(*(cur_data++), &zynq_i2c->data);
 		if (readl(&zynq_i2c->transfer_size) == ZYNQ_I2C_FIFO_DEPTH) {
-			if (!zynq_i2c_wait(ZYNQ_I2C_INTERRUPT_COMP)) {
+			if (!zynq_i2c_wait(zynq_i2c, ZYNQ_I2C_INTERRUPT_COMP)) {
 				/* Release the bus */
 				clrbits_le32(&zynq_i2c->control,
 					     ZYNQ_I2C_CONTROL_HOLD);
@@ -273,7 +283,7 @@ static int zynq_i2c_write(struct i2c_adapter *adap, u8 dev, uint addr,
 	/* All done... release the bus */
 	clrbits_le32(&zynq_i2c->control, ZYNQ_I2C_CONTROL_HOLD);
 	/* Wait for the address and data to be sent */
-	if (!zynq_i2c_wait(ZYNQ_I2C_INTERRUPT_COMP))
+	if (!zynq_i2c_wait(zynq_i2c, ZYNQ_I2C_INTERRUPT_COMP))
 		return -ETIMEDOUT;
 	return 0;
 }
@@ -291,3 +301,7 @@ U_BOOT_I2C_ADAP_COMPLETE(zynq_0, zynq_i2c_init, zynq_i2c_probe, zynq_i2c_read,
 			 zynq_i2c_write, zynq_i2c_set_bus_speed,
 			 CONFIG_SYS_I2C_ZYNQ_SPEED, CONFIG_SYS_I2C_ZYNQ_SLAVE,
 			 0)
+U_BOOT_I2C_ADAP_COMPLETE(zynq_1, zynq_i2c_init, zynq_i2c_probe, zynq_i2c_read,
+			 zynq_i2c_write, zynq_i2c_set_bus_speed,
+			 CONFIG_SYS_I2C_ZYNQ_SPEED, CONFIG_SYS_I2C_ZYNQ_SLAVE,
+			 1)