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
 
+++ /dev/null
-/*
- * 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 */
 
 
 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
 
--- /dev/null
+/*
+ * 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);
+}
 
 #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 */
 
 
 #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 */
 
 /*
 
 
 /*
  * 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
 #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 | \