Blackfin: overhaul i2c driver

The current Blackfin i2c driver does not work properly with certain devices
due to it breaking up transfers incorrectly.  This is a rewrite of the
driver and relocates it to the newer place in the source tree.

Also remove duplicated I2C speed defines in Blackfin board configs and
disable I2C slave address usage since it isn't implemented.

Signed-off-by: Mike Frysinger <vapier@gentoo.org>

7 files changed, 293 insertions, 470 deletions

diff --git a/cpu/blackfin/Makefile b/cpu/blackfin/Makefile
index 8fed4b422b7..b90fb486716 100644
--- a/cpu/blackfin/Makefile
+++ b/cpu/blackfin/Makefile
@@ -17,7 +17,7 @@ EXTRA    :=
 CEXTRA   := initcode.o
 SEXTRA   := start.o
 SOBJS    := interrupt.o cache.o
-COBJS    := cpu.o traps.o interrupts.o reset.o serial.o i2c.o watchdog.o
+COBJS    := cpu.o traps.o interrupts.o reset.o serial.o watchdog.o
 
 ifeq ($(CONFIG_BFIN_BOOT_MODE),BFIN_BOOT_BYPASS)
 COBJS    += initcode.o
diff --git a/cpu/blackfin/i2c.c b/cpu/blackfin/i2c.c
deleted file mode 100644
index 2a3e2238c36..00000000000
--- a/cpu/blackfin/i2c.c
+++ /dev/null
@@ -1,428 +0,0 @@
-/*
- * i2c.c - driver for Blackfin on-chip TWI/I2C
- *
- * Copyright (c) 2006-2008 Analog Devices Inc.
- *
- * Licensed under the GPL-2 or later.
- */
-
-#include <common.h>
-
-#ifdef CONFIG_HARD_I2C
-
-#include <asm/blackfin.h>
-#include <i2c.h>
-#include <asm/io.h>
-#include <asm/mach-common/bits/twi.h>
-
-/* Two-Wire Interface		(0xFFC01400 - 0xFFC014FF) */
-#ifdef TWI0_CLKDIV
-#define bfin_read_TWI_CLKDIV()               bfin_read_TWI0_CLKDIV()
-#define bfin_write_TWI_CLKDIV(val)           bfin_write_TWI0_CLKDIV(val)
-#define bfin_read_TWI_CONTROL()              bfin_read_TWI0_CONTROL()
-#define bfin_write_TWI_CONTROL(val)          bfin_write_TWI0_CONTROL(val)
-#define bfin_read_TWI_SLAVE_CTL()            bfin_read_TWI0_SLAVE_CTL()
-#define bfin_write_TWI_SLAVE_CTL(val)        bfin_write_TWI0_SLAVE_CTL(val)
-#define bfin_read_TWI_SLAVE_STAT()           bfin_read_TWI0_SLAVE_STAT()
-#define bfin_write_TWI_SLAVE_STAT(val)       bfin_write_TWI0_SLAVE_STAT(val)
-#define bfin_read_TWI_SLAVE_ADDR()           bfin_read_TWI0_SLAVE_ADDR()
-#define bfin_write_TWI_SLAVE_ADDR(val)       bfin_write_TWI0_SLAVE_ADDR(val)
-#define bfin_read_TWI_MASTER_CTL()           bfin_read_TWI0_MASTER_CTL()
-#define bfin_write_TWI_MASTER_CTL(val)       bfin_write_TWI0_MASTER_CTL(val)
-#define bfin_read_TWI_MASTER_STAT()          bfin_read_TWI0_MASTER_STAT()
-#define bfin_write_TWI_MASTER_STAT(val)      bfin_write_TWI0_MASTER_STAT(val)
-#define bfin_read_TWI_MASTER_ADDR()          bfin_read_TWI0_MASTER_ADDR()
-#define bfin_write_TWI_MASTER_ADDR(val)      bfin_write_TWI0_MASTER_ADDR(val)
-#define bfin_read_TWI_INT_STAT()             bfin_read_TWI0_INT_STAT()
-#define bfin_write_TWI_INT_STAT(val)         bfin_write_TWI0_INT_STAT(val)
-#define bfin_read_TWI_INT_MASK()             bfin_read_TWI0_INT_MASK()
-#define bfin_write_TWI_INT_MASK(val)         bfin_write_TWI0_INT_MASK(val)
-#define bfin_read_TWI_FIFO_CTL()             bfin_read_TWI0_FIFO_CTL()
-#define bfin_write_TWI_FIFO_CTL(val)         bfin_write_TWI0_FIFO_CTL(val)
-#define bfin_read_TWI_FIFO_STAT()            bfin_read_TWI0_FIFO_STAT()
-#define bfin_write_TWI_FIFO_STAT(val)        bfin_write_TWI0_FIFO_STAT(val)
-#define bfin_read_TWI_XMT_DATA8()            bfin_read_TWI0_XMT_DATA8()
-#define bfin_write_TWI_XMT_DATA8(val)        bfin_write_TWI0_XMT_DATA8(val)
-#define bfin_read_TWI_XMT_DATA_16()          bfin_read_TWI0_XMT_DATA16()
-#define bfin_write_TWI_XMT_DATA16(val)       bfin_write_TWI0_XMT_DATA16(val)
-#define bfin_read_TWI_RCV_DATA8()            bfin_read_TWI0_RCV_DATA8()
-#define bfin_write_TWI_RCV_DATA8(val)        bfin_write_TWI0_RCV_DATA8(val)
-#define bfin_read_TWI_RCV_DATA16()           bfin_read_TWI0_RCV_DATA16()
-#define bfin_write_TWI_RCV_DATA16(val)       bfin_write_TWI0_RCV_DATA16(val)
-#endif
-
-#ifdef DEBUG_I2C
-#define PRINTD(fmt,args...)	do {	\
-	DECLARE_GLOBAL_DATA_PTR;	\
-	if (gd->have_console)		\
-		printf(fmt ,##args);	\
-	} while (0)
-#else
-#define PRINTD(fmt,args...)
-#endif
-
-#ifndef CONFIG_TWICLK_KHZ
-#define CONFIG_TWICLK_KHZ	50
-#endif
-
-/* All transfers are described by this data structure */
-struct i2c_msg {
-	u16 addr;		/* slave address */
-	u16 flags;
-#define I2C_M_STOP		0x2
-#define I2C_M_RD		0x1
-	u16 len;		/* msg length */
-	u8 *buf;		/* pointer to msg data */
-};
-
-/**
- * i2c_reset: - reset the host controller
- */
-static void i2c_reset(void)
-{
-	/* Disable TWI */
-	bfin_write_TWI_CONTROL(0);
-	SSYNC();
-
-	/* Set TWI internal clock as 10MHz */
-	bfin_write_TWI_CONTROL(((get_sclk() / 1024 / 1024 + 5) / 10) & 0x7F);
-
-	/* Set Twi interface clock as specified */
-	if (CONFIG_TWICLK_KHZ > 400)
-		bfin_write_TWI_CLKDIV(((5 * 1024 / 400) << 8) | ((5 * 1024 /
-						400) & 0xFF));
-	else
-		bfin_write_TWI_CLKDIV(((5 * 1024 /
-					CONFIG_TWICLK_KHZ) << 8) | ((5 * 1024 /
-						CONFIG_TWICLK_KHZ)
-						& 0xFF));
-
-	/* Enable TWI */
-	bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() | TWI_ENA);
-	SSYNC();
-}
-
-int wait_for_completion(struct i2c_msg *msg, int timeout_count)
-{
-	unsigned short twi_int_stat;
-	unsigned short mast_stat;
-	int i;
-
-	for (i = 0; i < timeout_count; i++) {
-		twi_int_stat = bfin_read_TWI_INT_STAT();
-		mast_stat = bfin_read_TWI_MASTER_STAT();
-
-		if (XMTSERV & twi_int_stat) {
-			/* Transmit next data */
-			if (msg->len > 0) {
-				bfin_write_TWI_XMT_DATA8(*(msg->buf++));
-				msg->len--;
-			} else if (msg->flags & I2C_M_STOP)
-				bfin_write_TWI_MASTER_CTL
-				    (bfin_read_TWI_MASTER_CTL() | STOP);
-			SSYNC();
-			/* Clear status */
-			bfin_write_TWI_INT_STAT(XMTSERV);
-			SSYNC();
-			i = 0;
-		}
-		if (RCVSERV & twi_int_stat) {
-			if (msg->len > 0) {
-				/* Receive next data */
-				*(msg->buf++) = bfin_read_TWI_RCV_DATA8();
-				msg->len--;
-			} else if (msg->flags & I2C_M_STOP) {
-				bfin_write_TWI_MASTER_CTL
-				    (bfin_read_TWI_MASTER_CTL() | STOP);
-				SSYNC();
-			}
-			/* Clear interrupt source */
-			bfin_write_TWI_INT_STAT(RCVSERV);
-			SSYNC();
-			i = 0;
-		}
-		if (MERR & twi_int_stat) {
-			bfin_write_TWI_INT_STAT(MERR);
-			bfin_write_TWI_INT_MASK(0);
-			bfin_write_TWI_MASTER_STAT(0x3e);
-			bfin_write_TWI_MASTER_CTL(0);
-			SSYNC();
-			/*
-			 * if both err and complete int stats are set,
-			 * return proper results.
-			 */
-			if (MCOMP & twi_int_stat) {
-				bfin_write_TWI_INT_STAT(MCOMP);
-				bfin_write_TWI_INT_MASK(0);
-				bfin_write_TWI_MASTER_CTL(0);
-				SSYNC();
-				/*
-				 * If it is a quick transfer,
-				 * only address bug no data, not an err.
-				 */
-				if (msg->len == 0 && mast_stat & BUFRDERR)
-					return 0;
-				/*
-				 * If address not acknowledged return -3,
-				 * else return 0.
-				 */
-				else if (!(mast_stat & ANAK))
-					return 0;
-				else
-					return -3;
-			}
-			return -1;
-		}
-		if (MCOMP & twi_int_stat) {
-			bfin_write_TWI_INT_STAT(MCOMP);
-			SSYNC();
-			bfin_write_TWI_INT_MASK(0);
-			bfin_write_TWI_MASTER_CTL(0);
-			SSYNC();
-			return 0;
-		}
-	}
-	if (msg->flags & I2C_M_RD)
-		return -4;
-	else
-		return -2;
-}
-
-/**
- * i2c_transfer: - Transfer one byte over the i2c bus
- *
- * This function can tranfer a byte over the i2c bus in both directions.
- * It is used by the public API functions.
- *
- * @return:	 0: transfer successful
- *		-1: transfer fail
- *		-2: transmit timeout
- *		-3: ACK missing
- *		-4: receive timeout
- *		-5: controller not ready
- */
-int i2c_transfer(struct i2c_msg *msg)
-{
-	int ret = 0;
-	int timeout_count = 10000;
-	int len = msg->len;
-
-	if (!(bfin_read_TWI_CONTROL() & TWI_ENA)) {
-		ret = -5;
-		goto transfer_error;
-	}
-
-	while (bfin_read_TWI_MASTER_STAT() & BUSBUSY)
-		continue;
-
-	/* Set Transmit device address */
-	bfin_write_TWI_MASTER_ADDR(msg->addr);
-
-	/*
-	 * FIFO Initiation.
-	 * Data in FIFO should be discarded before start a new operation.
-	 */
-	bfin_write_TWI_FIFO_CTL(0x3);
-	SSYNC();
-	bfin_write_TWI_FIFO_CTL(0);
-	SSYNC();
-
-	if (!(msg->flags & I2C_M_RD)) {
-		/* Transmit first data */
-		if (msg->len > 0) {
-			PRINTD("1 in i2c_transfer: buf=%d, len=%d\n", *msg->buf,
-			       len);
-			bfin_write_TWI_XMT_DATA8(*(msg->buf++));
-			msg->len--;
-			SSYNC();
-		}
-	}
-
-	/* clear int stat */
-	bfin_write_TWI_INT_STAT(MERR | MCOMP | XMTSERV | RCVSERV);
-
-	/* Interrupt mask . Enable XMT, RCV interrupt */
-	bfin_write_TWI_INT_MASK(MCOMP | MERR |
-			((msg->flags & I2C_M_RD) ? RCVSERV : XMTSERV));
-	SSYNC();
-
-	if (len > 0 && len <= 255)
-		bfin_write_TWI_MASTER_CTL((len << 6));
-	else if (msg->len > 255) {
-		bfin_write_TWI_MASTER_CTL((0xff << 6));
-		msg->flags &= I2C_M_STOP;
-	} else
-		bfin_write_TWI_MASTER_CTL(0);
-
-	/* Master enable */
-	bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() | MEN |
-			((msg->flags & I2C_M_RD)
-			 ? MDIR : 0) | ((CONFIG_TWICLK_KHZ >
-					 100) ? FAST : 0));
-	SSYNC();
-
-	ret = wait_for_completion(msg, timeout_count);
-	PRINTD("3 in i2c_transfer: ret=%d\n", ret);
-
- transfer_error:
-	switch (ret) {
-	case 1:
-		PRINTD(("i2c_transfer: error: transfer fail\n"));
-		break;
-	case 2:
-		PRINTD(("i2c_transfer: error: transmit timeout\n"));
-		break;
-	case 3:
-		PRINTD(("i2c_transfer: error: ACK missing\n"));
-		break;
-	case 4:
-		PRINTD(("i2c_transfer: error: receive timeout\n"));
-		break;
-	case 5:
-		PRINTD(("i2c_transfer: error: controller not ready\n"));
-		i2c_reset();
-		break;
-	default:
-		break;
-	}
-	return ret;
-
-}
-
-/* ---------------------------------------------------------------------*/
-/* API Functions							*/
-/* ---------------------------------------------------------------------*/
-
-void i2c_init(int speed, int slaveaddr)
-{
-	i2c_reset();
-}
-
-/**
- * i2c_probe: - Test if a chip answers for a given i2c address
- *
- * @chip:	address of the chip which is searched for
- * @return:	0 if a chip was found, -1 otherwhise
- */
-
-int i2c_probe(uchar chip)
-{
-	struct i2c_msg msg;
-	u8 probebuf;
-
-	i2c_reset();
-
-	probebuf = 0;
-	msg.addr = chip;
-	msg.flags = 0;
-	msg.len = 1;
-	msg.buf = &probebuf;
-	if (i2c_transfer(&msg))
-		return -1;
-
-	msg.addr = chip;
-	msg.flags = I2C_M_RD;
-	msg.len = 1;
-	msg.buf = &probebuf;
-	if (i2c_transfer(&msg))
-		return -1;
-
-	return 0;
-}
-
-/**
- *   i2c_read: - Read multiple bytes from an i2c device
- *
- *   chip:    I2C chip address, range 0..127
- *   addr:    Memory (register) address within the chip
- *   alen:    Number of bytes to use for addr (typically 1, 2 for larger
- *		memories, 0 for register type devices with only one
- *		register)
- *   buffer:  Where to read/write the data
- *   len:     How many bytes to read/write
- *
- *   Returns: 0 on success, not 0 on failure
- */
-
-int i2c_read(uchar chip, uint addr, int alen, uchar * buffer, int len)
-{
-	struct i2c_msg msg;
-	u8 addr_bytes[3];	/* lowest...highest byte of data address */
-
-	PRINTD("i2c_read: chip=0x%x, addr=0x%x, alen=0x%x, len=0x%x\n", chip,
-			addr, alen, len);
-
-	if (alen > 0) {
-		addr_bytes[0] = (u8) ((addr >> 0) & 0x000000FF);
-		addr_bytes[1] = (u8) ((addr >> 8) & 0x000000FF);
-		addr_bytes[2] = (u8) ((addr >> 16) & 0x000000FF);
-		msg.addr = chip;
-		msg.flags = 0;
-		msg.len = alen;
-		msg.buf = addr_bytes;
-		if (i2c_transfer(&msg))
-			return -1;
-	}
-
-	/* start read sequence */
-	PRINTD(("i2c_read: start read sequence\n"));
-	msg.addr = chip;
-	msg.flags = I2C_M_RD;
-	msg.len = len;
-	msg.buf = buffer;
-	if (i2c_transfer(&msg))
-		return -1;
-
-	return 0;
-}
-
-/**
- *   i2c_write: -  Write multiple bytes to an i2c device
- *
- *   chip:    I2C chip address, range 0..127
- *   addr:    Memory (register) address within the chip
- *   alen:    Number of bytes to use for addr (typically 1, 2 for larger
- *		memories, 0 for register type devices with only one
- *		register)
- *   buffer:  Where to read/write the data
- *   len:     How many bytes to read/write
- *
- *   Returns: 0 on success, not 0 on failure
- */
-
-int i2c_write(uchar chip, uint addr, int alen, uchar * buffer, int len)
-{
-	struct i2c_msg msg;
-	u8 addr_bytes[3];	/* lowest...highest byte of data address */
-
-	PRINTD
-		("i2c_write: chip=0x%x, addr=0x%x, alen=0x%x, len=0x%x, buf0=0x%x\n",
-		 chip, addr, alen, len, buffer[0]);
-
-	/* chip address write */
-	if (alen > 0) {
-		addr_bytes[0] = (u8) ((addr >> 0) & 0x000000FF);
-		addr_bytes[1] = (u8) ((addr >> 8) & 0x000000FF);
-		addr_bytes[2] = (u8) ((addr >> 16) & 0x000000FF);
-		msg.addr = chip;
-		msg.flags = 0;
-		msg.len = alen;
-		msg.buf = addr_bytes;
-		if (i2c_transfer(&msg))
-			return -1;
-	}
-
-	/* start read sequence */
-	PRINTD(("i2c_write: start write sequence\n"));
-	msg.addr = chip;
-	msg.flags = 0;
-	msg.len = len;
-	msg.buf = buffer;
-	if (i2c_transfer(&msg))
-		return -1;
-
-	return 0;
-
-}
-
-#endif /* CONFIG_HARD_I2C */
diff --git a/drivers/i2c/Makefile b/drivers/i2c/Makefile
index 6079c05ade3..08cb91d600e 100644
--- a/drivers/i2c/Makefile
+++ b/drivers/i2c/Makefile
@@ -25,6 +25,7 @@ include $(TOPDIR)/config.mk
 
 LIB	:= $(obj)libi2c.a
 
+COBJS-$(CONFIG_BFIN_TWI_I2C) += bfin-twi_i2c.o
 COBJS-$(CONFIG_FSL_I2C) += fsl_i2c.o
 COBJS-$(CONFIG_I2C_MXC) += mxc_i2c.o
 COBJS-$(CONFIG_DRIVER_OMAP1510_I2C) += omap1510_i2c.o
diff --git a/drivers/i2c/bfin-twi_i2c.c b/drivers/i2c/bfin-twi_i2c.c
new file mode 100644
index 00000000000..cfe55cd1d28
--- /dev/null
+++ b/drivers/i2c/bfin-twi_i2c.c
@@ -0,0 +1,285 @@
+/*
+ * i2c.c - driver for Blackfin on-chip TWI/I2C
+ *
+ * Copyright (c) 2006-2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <common.h>
+#include <i2c.h>
+
+#include <asm/blackfin.h>
+#include <asm/mach-common/bits/twi.h>
+
+#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", \
+		bfin_read_TWI_MASTER_STAT(), bfin_read_TWI_FIFO_STAT(), bfin_read_TWI_INT_STAT(), \
+		__func__, __LINE__, ## args)
+
+#ifdef TWI0_CLKDIV
+#define bfin_write_TWI_CLKDIV(val)           bfin_write_TWI0_CLKDIV(val)
+#define bfin_write_TWI_CONTROL(val)          bfin_write_TWI0_CONTROL(val)
+#define bfin_read_TWI_CONTROL(val)           bfin_read_TWI0_CONTROL(val)
+#define bfin_write_TWI_MASTER_ADDR(val)      bfin_write_TWI0_MASTER_ADDR(val)
+#define bfin_write_TWI_XMT_DATA8(val)        bfin_write_TWI0_XMT_DATA8(val)
+#define bfin_read_TWI_RCV_DATA8()            bfin_read_TWI0_RCV_DATA8()
+#define bfin_read_TWI_INT_STAT()             bfin_read_TWI0_INT_STAT()
+#define bfin_write_TWI_INT_STAT(val)         bfin_write_TWI0_INT_STAT(val)
+#define bfin_read_TWI_MASTER_STAT()          bfin_read_TWI0_MASTER_STAT()
+#define bfin_write_TWI_MASTER_STAT(val)      bfin_write_TWI0_MASTER_STAT(val)
+#define bfin_read_TWI_MASTER_CTL()           bfin_read_TWI0_MASTER_CTL()
+#define bfin_write_TWI_MASTER_CTL(val)       bfin_write_TWI0_MASTER_CTL(val)
+#define bfin_write_TWI_INT_MASK(val)         bfin_write_TWI0_INT_MASK(val)
+#define bfin_write_TWI_FIFO_CTL(val)         bfin_write_TWI0_FIFO_CTL(val)
+#endif
+
+#ifdef CONFIG_TWICLK_KHZ
+# error do not define CONFIG_TWICLK_KHZ ... use CONFIG_SYS_I2C_SPEED
+#endif
+#if CONFIG_SYS_I2C_SPEED > 400000
+# error The Blackfin I2C hardware can only operate at 400KHz max
+#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 */
+};
+
+/**
+ * wait_for_completion - manage the actual i2c transfer
+ *	@msg: the i2c msg
+ */
+static int wait_for_completion(struct i2c_msg *msg)
+{
+	uint16_t int_stat;
+
+	while (!ctrlc()) {
+		int_stat = bfin_read_TWI_INT_STAT();
+
+		if (int_stat & XMTSERV) {
+			debugi("processing XMTSERV");
+			bfin_write_TWI_INT_STAT(XMTSERV);
+			SSYNC();
+			if (msg->alen) {
+				bfin_write_TWI_XMT_DATA8(*(msg->abuf++));
+				--msg->alen;
+			} else if (!(msg->flags & I2C_M_COMBO) && msg->len) {
+				bfin_write_TWI_XMT_DATA8(*(msg->buf++));
+				--msg->len;
+			} else {
+				bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() |
+					(msg->flags & I2C_M_COMBO ? RSTART | MDIR : STOP));
+				SSYNC();
+			}
+		}
+		if (int_stat & RCVSERV) {
+			debugi("processing RCVSERV");
+			bfin_write_TWI_INT_STAT(RCVSERV);
+			SSYNC();
+			if (msg->len) {
+				*(msg->buf++) = bfin_read_TWI_RCV_DATA8();
+				--msg->len;
+			} else if (msg->flags & I2C_M_STOP) {
+				bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() | STOP);
+				SSYNC();
+			}
+		}
+		if (int_stat & MERR) {
+			debugi("processing MERR");
+			bfin_write_TWI_INT_STAT(MERR);
+			SSYNC();
+			break;
+		}
+		if (int_stat & MCOMP) {
+			debugi("processing MCOMP");
+			bfin_write_TWI_INT_STAT(MCOMP);
+			SSYNC();
+			if (msg->flags & I2C_M_COMBO && msg->len) {
+				bfin_write_TWI_MASTER_CTL((bfin_read_TWI_MASTER_CTL() & ~RSTART) |
+					(min(msg->len, 0xff) << 6) | MEN | MDIR);
+				SSYNC();
+			} else
+				break;
+		}
+	}
+
+	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)
+{
+	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,
+	};
+	int ret;
+
+	dmemset(buffer, 0xff, len);
+	debugi("chip=0x%x addr=0x%02x alen=%i buf[0]=0x%02x len=%i flags=0x%02x[%s] ",
+		chip, addr, alen, buffer[0], len, flags, (flags & I2C_M_READ ? "rd" : "wr"));
+
+	/* wait for things to settle */
+	while (bfin_read_TWI_MASTER_STAT() & BUSBUSY)
+		if (ctrlc())
+			return 1;
+
+	/* Set Transmit device address */
+	bfin_write_TWI_MASTER_ADDR(chip);
+
+	/* Clear the FIFO before starting things */
+	bfin_write_TWI_FIFO_CTL(XMTFLUSH | RCVFLUSH);
+	SSYNC();
+	bfin_write_TWI_FIFO_CTL(0);
+	SSYNC();
+
+	/* prime the pump */
+	if (msg.alen) {
+		len = msg.alen;
+		debugi("first byte=0x%02x", *msg.abuf);
+		bfin_write_TWI_XMT_DATA8(*(msg.abuf++));
+		--msg.alen;
+	} else if (!(msg.flags & I2C_M_READ) && msg.len) {
+		debugi("first byte=0x%02x", *msg.buf);
+		bfin_write_TWI_XMT_DATA8(*(msg.buf++));
+		--msg.len;
+	}
+
+	/* clear int stat */
+	bfin_write_TWI_MASTER_STAT(-1);
+	bfin_write_TWI_INT_STAT(-1);
+	bfin_write_TWI_INT_MASK(0);
+	SSYNC();
+
+	/* Master enable */
+	bfin_write_TWI_MASTER_CTL(
+			(bfin_read_TWI_MASTER_CTL() & FAST) |
+			(min(len, 0xff) << 6) | MEN |
+			((msg.flags & I2C_M_READ) ? MDIR : 0)
+	);
+	SSYNC();
+	debugi("CTL=0x%04x", bfin_read_TWI_MASTER_CTL());
+
+	/* process the rest */
+	ret = wait_for_completion(&msg);
+	debugi("ret=%d", ret);
+
+	if (ret) {
+		bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() & ~MEN);
+		bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() & ~TWI_ENA);
+		SSYNC();
+		bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() | TWI_ENA);
+		SSYNC();
+	}
+
+	return ret;
+}
+
+/*
+ * 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)
+{
+	uint8_t prescale = ((get_sclk() / 1024 / 1024 + 5) / 10) & 0x7F;
+
+	/* Set TWI internal clock as 10MHz */
+	bfin_write_TWI_CONTROL(prescale);
+
+	/* Set TWI interface clock as specified */
+	bfin_write_TWI_CLKDIV(
+		((5 * 1024 / (speed / 1000)) << 8) |
+		((5 * 1024 / (speed / 1000)) & 0xFF)
+	);
+
+	/* Don't turn it on */
+	bfin_write_TWI_MASTER_CTL(speed > 100000 ? FAST : 0);
+
+	/* But enable it */
+	bfin_write_TWI_CONTROL(TWI_ENA | prescale);
+	SSYNC();
+
+	debugi("CONTROL:0x%04x CLKDIV:0x%04x",
+		bfin_read_TWI_CONTROL(), bfin_read_TWI_CLKDIV());
+
+#if CONFIG_SYS_I2C_SLAVE
+# error I2C slave support not tested/supported
+	/* If they want us as a slave, do it */
+	if (slaveaddr) {
+		bfin_write_TWI_SLAVE_ADDR(slaveaddr);
+		bfin_write_TWI_SLAVE_CTL(SEN);
+	}
+#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 to store data read from 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);
+}
diff --git a/include/configs/bf533-ezkit.h b/include/configs/bf533-ezkit.h
index e871737177a..48c0252e46e 100644
--- a/include/configs/bf533-ezkit.h
+++ b/include/configs/bf533-ezkit.h
@@ -198,7 +198,7 @@
 #define I2C_DELAY	udelay(5)	/* 1/4 I2C clock duration */
 
 #define CONFIG_SYS_I2C_SPEED		50000
-#define CONFIG_SYS_I2C_SLAVE		0xFE
+#define CONFIG_SYS_I2C_SLAVE		0
 
 #define CONFIG_SYS_BOOTM_LEN		0x4000000	/* Large Image Length, set to 64 Meg */
 
diff --git a/include/configs/bf533-stamp.h b/include/configs/bf533-stamp.h
index 5ad99a2fd49..ee41c7e78f8 100644
--- a/include/configs/bf533-stamp.h
+++ b/include/configs/bf533-stamp.h
@@ -300,7 +300,7 @@
 #define I2C_DELAY		udelay(5)	/* 1/4 I2C clock duration */
 
 #define CONFIG_SYS_I2C_SPEED		50000
-#define CONFIG_SYS_I2C_SLAVE		0xFE
+#define CONFIG_SYS_I2C_SLAVE		0
 #endif /* CONFIG_SOFT_I2C */
 
 /*
diff --git a/include/configs/bf537-stamp.h b/include/configs/bf537-stamp.h
index ac5aaa59aed..27567faab28 100644
--- a/include/configs/bf537-stamp.h
+++ b/include/configs/bf537-stamp.h
@@ -305,13 +305,11 @@
 
 /*
  * I2C settings
- * By default PF1 is used as SDA and PF0 as SCL on the Stamp board
  */
-/* #define CONFIG_SOFT_I2C	1*/	/* I2C bit-banged */
-#define CONFIG_HARD_I2C		1	/* I2C TWI */
-#if defined CONFIG_HARD_I2C
-#define CONFIG_TWICLK_KHZ	50
-#endif
+#define CONFIG_HARD_I2C		1
+#define CONFIG_BFIN_TWI_I2C	1
+#define CFG_I2C_SPEED		50000
+#define CFG_I2C_SLAVE		0
 
 #define CONFIG_EBIU_SDRRC_VAL  0x306
 #define CONFIG_EBIU_SDGCTL_VAL 0x91114d
@@ -321,39 +319,6 @@
 #define CONFIG_EBIU_AMBCTL0_VAL		0x7BB07BB0
 #define CONFIG_EBIU_AMBCTL1_VAL		0xFFC27BB0
 
-#if defined CONFIG_SOFT_I2C
-/*
- * Software (bit-bang) I2C driver configuration
- */
-#define PF_SCL			PF0
-#define PF_SDA			PF1
-
-#define I2C_INIT		(*pFIO_DIR |=  PF_SCL); asm("ssync;")
-#define I2C_ACTIVE		(*pFIO_DIR |=  PF_SDA); *pFIO_INEN &= ~PF_SDA; asm("ssync;")
-#define I2C_TRISTATE		(*pFIO_DIR &= ~PF_SDA); *pFIO_INEN |= PF_SDA; asm("ssync;")
-#define I2C_READ		((volatile)(*pFIO_FLAG_D & PF_SDA) != 0); asm("ssync;")
-#define I2C_SDA(bit)		if(bit) { \
-					*pFIO_FLAG_S = PF_SDA; \
-					asm("ssync;"); \
-					} \
-				else    { \
-					*pFIO_FLAG_C = PF_SDA; \
-					asm("ssync;"); \
-					}
-#define I2C_SCL(bit)		if(bit) { \
-					*pFIO_FLAG_S = PF_SCL; \
-					asm("ssync;"); \
-					} \
-				else    { \
-					*pFIO_FLAG_C = PF_SCL; \
-					asm("ssync;"); \
-					}
-#define I2C_DELAY		udelay(5)	/* 1/4 I2C clock duration */
-#endif
-
-#define CONFIG_SYS_I2C_SPEED		50000
-#define CONFIG_SYS_I2C_SLAVE		0xFE
-
 /* 0xFF, 0x7BB07BB0, 0x22547BB0 */
 /* #define AMGCTLVAL		(AMBEN_P0 | AMBEN_P1 | AMBEN_P2 | AMCKEN)
 #define AMBCTL0VAL		(B1WAT_7 | B1RAT_11 | B1HT_2 | B1ST_3 | B1TT_4 | ~B1RDYPOL | \