i2c-bfin-twi: Add platform_resource interface to support multi-port TWI controllers
authorBryan Wu <cooloney@kernel.org>
Tue, 22 Apr 2008 20:16:48 +0000 (22:16 +0200)
committerJean Delvare <khali@hyperion.delvare>
Tue, 22 Apr 2008 20:16:48 +0000 (22:16 +0200)
 - Dynamic alloc the resource of TWI driver data according to board information
 - TWI register read/write accessor based on dynamic regs_base
 - Support TWI0/TWI1 for BF54x

Signed-off-by: Bryan Wu <cooloney@kernel.org>
Signed-off-by: Jean Delvare <khali@linux-fr.org>
drivers/i2c/busses/Kconfig
drivers/i2c/busses/i2c-bfin-twi.c

index 495c971ce5bfa6a3b64376a36c25440f56321074..7d538ddfd37ec001356a571e5584f853ce1f6971 100644 (file)
@@ -100,7 +100,7 @@ config I2C_AU1550
 
 config I2C_BLACKFIN_TWI
        tristate "Blackfin TWI I2C support"
-       depends on BF534 || BF536 || BF537
+       depends on BF534 || BF536 || BF537 || BF54x
        help
          This is the TWI I2C device driver for Blackfin 534/536/537/54x.
          This driver can also be built as a module.  If so, the module
index 86956a8f0ae5324748239907c60f72c380304f2e..f2b558f17ca21a78f27f44270c03f4617219ca7b 100644 (file)
@@ -62,43 +62,66 @@ struct bfin_twi_iface {
        struct i2c_msg          *pmsg;
        int                     msg_num;
        int                     cur_msg;
+       void __iomem            *regs_base;
 };
 
-static struct bfin_twi_iface twi_iface;
+
+#define DEFINE_TWI_REG(reg, off) \
+static inline u16 read_##reg(struct bfin_twi_iface *iface) \
+       { return bfin_read16(iface->regs_base + (off)); } \
+static inline void write_##reg(struct bfin_twi_iface *iface, u16 v) \
+       { bfin_write16(iface->regs_base + (off), v); }
+
+DEFINE_TWI_REG(CLKDIV, 0x00)
+DEFINE_TWI_REG(CONTROL, 0x04)
+DEFINE_TWI_REG(SLAVE_CTL, 0x08)
+DEFINE_TWI_REG(SLAVE_STAT, 0x0C)
+DEFINE_TWI_REG(SLAVE_ADDR, 0x10)
+DEFINE_TWI_REG(MASTER_CTL, 0x14)
+DEFINE_TWI_REG(MASTER_STAT, 0x18)
+DEFINE_TWI_REG(MASTER_ADDR, 0x1C)
+DEFINE_TWI_REG(INT_STAT, 0x20)
+DEFINE_TWI_REG(INT_MASK, 0x24)
+DEFINE_TWI_REG(FIFO_CTL, 0x28)
+DEFINE_TWI_REG(FIFO_STAT, 0x2C)
+DEFINE_TWI_REG(XMT_DATA8, 0x80)
+DEFINE_TWI_REG(XMT_DATA16, 0x84)
+DEFINE_TWI_REG(RCV_DATA8, 0x88)
+DEFINE_TWI_REG(RCV_DATA16, 0x8C)
 
 static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface)
 {
-       unsigned short twi_int_status = bfin_read_TWI_INT_STAT();
-       unsigned short mast_stat = bfin_read_TWI_MASTER_STAT();
+       unsigned short twi_int_status = read_INT_STAT(iface);
+       unsigned short mast_stat = read_MASTER_STAT(iface);
 
        if (twi_int_status & XMTSERV) {
                /* Transmit next data */
                if (iface->writeNum > 0) {
-                       bfin_write_TWI_XMT_DATA8(*(iface->transPtr++));
+                       write_XMT_DATA8(iface, *(iface->transPtr++));
                        iface->writeNum--;
                }
                /* start receive immediately after complete sending in
                 * combine mode.
                 */
                else if (iface->cur_mode == TWI_I2C_MODE_COMBINED)
-                       bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
-                               | MDIR | RSTART);
+                       write_MASTER_CTL(iface,
+                               read_MASTER_CTL(iface) | MDIR | RSTART);
                else if (iface->manual_stop)
-                       bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
-                               | STOP);
+                       write_MASTER_CTL(iface,
+                               read_MASTER_CTL(iface) | STOP);
                else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
                                iface->cur_msg+1 < iface->msg_num)
-                       bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
-                               | RSTART);
+                       write_MASTER_CTL(iface,
+                               read_MASTER_CTL(iface) | RSTART);
                SSYNC();
                /* Clear status */
-               bfin_write_TWI_INT_STAT(XMTSERV);
+               write_INT_STAT(iface, XMTSERV);
                SSYNC();
        }
        if (twi_int_status & RCVSERV) {
                if (iface->readNum > 0) {
                        /* Receive next data */
-                       *(iface->transPtr) = bfin_read_TWI_RCV_DATA8();
+                       *(iface->transPtr) = read_RCV_DATA8(iface);
                        if (iface->cur_mode == TWI_I2C_MODE_COMBINED) {
                                /* Change combine mode into sub mode after
                                 * read first data.
@@ -113,33 +136,33 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface)
                        iface->transPtr++;
                        iface->readNum--;
                } else if (iface->manual_stop) {
-                       bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
-                               | STOP);
+                       write_MASTER_CTL(iface,
+                               read_MASTER_CTL(iface) | STOP);
                        SSYNC();
                } else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
                                iface->cur_msg+1 < iface->msg_num) {
-                       bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL()
-                               | RSTART);
+                       write_MASTER_CTL(iface,
+                               read_MASTER_CTL(iface) | RSTART);
                        SSYNC();
                }
                /* Clear interrupt source */
-               bfin_write_TWI_INT_STAT(RCVSERV);
+               write_INT_STAT(iface, RCVSERV);
                SSYNC();
        }
        if (twi_int_status & MERR) {
-               bfin_write_TWI_INT_STAT(MERR);
-               bfin_write_TWI_INT_MASK(0);
-               bfin_write_TWI_MASTER_STAT(0x3e);
-               bfin_write_TWI_MASTER_CTL(0);
+               write_INT_STAT(iface, MERR);
+               write_INT_MASK(iface, 0);
+               write_MASTER_STAT(iface, 0x3e);
+               write_MASTER_CTL(iface, 0);
                SSYNC();
                iface->result = -EIO;
                /* if both err and complete int stats are set, return proper
                 * results.
                 */
                if (twi_int_status & MCOMP) {
-                       bfin_write_TWI_INT_STAT(MCOMP);
-                       bfin_write_TWI_INT_MASK(0);
-                       bfin_write_TWI_MASTER_CTL(0);
+                       write_INT_STAT(iface, MCOMP);
+                       write_INT_MASK(iface, 0);
+                       write_MASTER_CTL(iface, 0);
                        SSYNC();
                        /* If it is a quick transfer, only address bug no data,
                         * not an err, return 1.
@@ -156,7 +179,7 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface)
                return;
        }
        if (twi_int_status & MCOMP) {
-               bfin_write_TWI_INT_STAT(MCOMP);
+               write_INT_STAT(iface, MCOMP);
                SSYNC();
                if (iface->cur_mode == TWI_I2C_MODE_COMBINED) {
                        if (iface->readNum == 0) {
@@ -165,23 +188,22 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface)
                                 */
                                iface->readNum = 1;
                                iface->manual_stop = 1;
-                               bfin_write_TWI_MASTER_CTL(
-                                       bfin_read_TWI_MASTER_CTL()
-                                       | (0xff << 6));
+                               write_MASTER_CTL(iface,
+                                       read_MASTER_CTL(iface) | (0xff << 6));
                        } else {
                                /* set the readd number in other
                                 * combine mode.
                                 */
-                               bfin_write_TWI_MASTER_CTL(
-                                       (bfin_read_TWI_MASTER_CTL() &
+                               write_MASTER_CTL(iface,
+                                       (read_MASTER_CTL(iface) &
                                        (~(0xff << 6))) |
-                                       ( iface->readNum << 6));
+                                       (iface->readNum << 6));
                        }
                        /* remove restart bit and enable master receive */
-                       bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() &
-                               ~RSTART);
-                       bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() |
-                               MEN | MDIR);
+                       write_MASTER_CTL(iface,
+                               read_MASTER_CTL(iface) & ~RSTART);
+                       write_MASTER_CTL(iface,
+                               read_MASTER_CTL(iface) | MEN | MDIR);
                        SSYNC();
                } else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
                                iface->cur_msg+1 < iface->msg_num) {
@@ -190,7 +212,7 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface)
                        iface->writeNum = iface->readNum =
                                iface->pmsg[iface->cur_msg].len;
                        /* Set Transmit device address */
-                       bfin_write_TWI_MASTER_ADDR(
+                       write_MASTER_ADDR(iface,
                                iface->pmsg[iface->cur_msg].addr);
                        if (iface->pmsg[iface->cur_msg].flags & I2C_M_RD)
                                iface->read_write = I2C_SMBUS_READ;
@@ -198,7 +220,7 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface)
                                iface->read_write = I2C_SMBUS_WRITE;
                                /* Transmit first data */
                                if (iface->writeNum > 0) {
-                                       bfin_write_TWI_XMT_DATA8(
+                                       write_XMT_DATA8(iface,
                                                *(iface->transPtr++));
                                        iface->writeNum--;
                                        SSYNC();
@@ -206,23 +228,23 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface)
                        }
 
                        if (iface->pmsg[iface->cur_msg].len <= 255)
-                               bfin_write_TWI_MASTER_CTL(
+                               write_MASTER_CTL(iface,
                                iface->pmsg[iface->cur_msg].len << 6);
                        else {
-                               bfin_write_TWI_MASTER_CTL(0xff << 6);
+                               write_MASTER_CTL(iface, 0xff << 6);
                                iface->manual_stop = 1;
                        }
                        /* remove restart bit and enable master receive */
-                       bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() &
-                               ~RSTART);
-                       bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() |
+                       write_MASTER_CTL(iface,
+                               read_MASTER_CTL(iface) & ~RSTART);
+                       write_MASTER_CTL(iface, read_MASTER_CTL(iface) |
                                MEN | ((iface->read_write == I2C_SMBUS_READ) ?
                                MDIR : 0));
                        SSYNC();
                } else {
                        iface->result = 1;
-                       bfin_write_TWI_INT_MASK(0);
-                       bfin_write_TWI_MASTER_CTL(0);
+                       write_INT_MASK(iface, 0);
+                       write_MASTER_CTL(iface, 0);
                        SSYNC();
                        complete(&iface->complete);
                }
@@ -272,12 +294,11 @@ static int bfin_twi_master_xfer(struct i2c_adapter *adap,
        struct i2c_msg *pmsg;
        int rc = 0;
 
-       if (!(bfin_read_TWI_CONTROL() & TWI_ENA))
+       if (!(read_CONTROL(iface) & TWI_ENA))
                return -ENXIO;
 
-       while (bfin_read_TWI_MASTER_STAT() & BUSBUSY) {
+       while (read_MASTER_STAT(iface) & BUSBUSY)
                yield();
-       }
 
        iface->pmsg = msgs;
        iface->msg_num = num;
@@ -296,14 +317,14 @@ static int bfin_twi_master_xfer(struct i2c_adapter *adap,
        iface->result = 0;
        iface->timeout_count = 10;
        /* Set Transmit device address */
-       bfin_write_TWI_MASTER_ADDR(pmsg->addr);
+       write_MASTER_ADDR(iface, pmsg->addr);
 
        /* FIFO Initiation. Data in FIFO should be
         *  discarded before start a new operation.
         */
-       bfin_write_TWI_FIFO_CTL(0x3);
+       write_FIFO_CTL(iface, 0x3);
        SSYNC();
-       bfin_write_TWI_FIFO_CTL(0);
+       write_FIFO_CTL(iface, 0);
        SSYNC();
 
        if (pmsg->flags & I2C_M_RD)
@@ -312,23 +333,23 @@ static int bfin_twi_master_xfer(struct i2c_adapter *adap,
                iface->read_write = I2C_SMBUS_WRITE;
                /* Transmit first data */
                if (iface->writeNum > 0) {
-                       bfin_write_TWI_XMT_DATA8(*(iface->transPtr++));
+                       write_XMT_DATA8(iface, *(iface->transPtr++));
                        iface->writeNum--;
                        SSYNC();
                }
        }
 
        /* clear int stat */
-       bfin_write_TWI_INT_STAT(MERR | MCOMP | XMTSERV | RCVSERV);
+       write_INT_STAT(iface, MERR | MCOMP | XMTSERV | RCVSERV);
 
        /* Interrupt mask . Enable XMT, RCV interrupt */
-       bfin_write_TWI_INT_MASK(MCOMP | MERR | RCVSERV | XMTSERV);
+       write_INT_MASK(iface, MCOMP | MERR | RCVSERV | XMTSERV);
        SSYNC();
 
        if (pmsg->len <= 255)
-               bfin_write_TWI_MASTER_CTL(pmsg->len << 6);
+               write_MASTER_CTL(iface, pmsg->len << 6);
        else {
-               bfin_write_TWI_MASTER_CTL(0xff << 6);
+               write_MASTER_CTL(iface, 0xff << 6);
                iface->manual_stop = 1;
        }
 
@@ -336,7 +357,7 @@ static int bfin_twi_master_xfer(struct i2c_adapter *adap,
        add_timer(&iface->timeout_timer);
 
        /* Master enable */
-       bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() | MEN |
+       write_MASTER_CTL(iface, read_MASTER_CTL(iface) | MEN |
                ((iface->read_write == I2C_SMBUS_READ) ? MDIR : 0) |
                ((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ > 100) ? FAST : 0));
        SSYNC();
@@ -362,12 +383,11 @@ int bfin_twi_smbus_xfer(struct i2c_adapter *adap, u16 addr,
        struct bfin_twi_iface *iface = adap->algo_data;
        int rc = 0;
 
-       if (!(bfin_read_TWI_CONTROL() & TWI_ENA))
+       if (!(read_CONTROL(iface) & TWI_ENA))
                return -ENXIO;
 
-       while (bfin_read_TWI_MASTER_STAT() & BUSBUSY) {
+       while (read_MASTER_STAT(iface) & BUSBUSY)
                yield();
-       }
 
        iface->writeNum = 0;
        iface->readNum = 0;
@@ -439,15 +459,15 @@ int bfin_twi_smbus_xfer(struct i2c_adapter *adap, u16 addr,
        /* FIFO Initiation. Data in FIFO should be discarded before
         * start a new operation.
         */
-       bfin_write_TWI_FIFO_CTL(0x3);
+       write_FIFO_CTL(iface, 0x3);
        SSYNC();
-       bfin_write_TWI_FIFO_CTL(0);
+       write_FIFO_CTL(iface, 0);
 
        /* clear int stat */
-       bfin_write_TWI_INT_STAT(MERR|MCOMP|XMTSERV|RCVSERV);
+       write_INT_STAT(iface, MERR | MCOMP | XMTSERV | RCVSERV);
 
        /* Set Transmit device address */
-       bfin_write_TWI_MASTER_ADDR(addr);
+       write_MASTER_ADDR(iface, addr);
        SSYNC();
 
        iface->timeout_timer.expires = jiffies + POLL_TIMEOUT;
@@ -455,60 +475,64 @@ int bfin_twi_smbus_xfer(struct i2c_adapter *adap, u16 addr,
 
        switch (iface->cur_mode) {
        case TWI_I2C_MODE_STANDARDSUB:
-               bfin_write_TWI_XMT_DATA8(iface->command);
-               bfin_write_TWI_INT_MASK(MCOMP | MERR |
+               write_XMT_DATA8(iface, iface->command);
+               write_INT_MASK(iface, MCOMP | MERR |
                        ((iface->read_write == I2C_SMBUS_READ) ?
                        RCVSERV : XMTSERV));
                SSYNC();
 
                if (iface->writeNum + 1 <= 255)
-                       bfin_write_TWI_MASTER_CTL((iface->writeNum + 1) << 6);
+                       write_MASTER_CTL(iface, (iface->writeNum + 1) << 6);
                else {
-                       bfin_write_TWI_MASTER_CTL(0xff << 6);
+                       write_MASTER_CTL(iface, 0xff << 6);
                        iface->manual_stop = 1;
                }
                /* Master enable */
-               bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() | MEN |
+               write_MASTER_CTL(iface, read_MASTER_CTL(iface) | MEN |
                        ((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ>100) ? FAST : 0));
                break;
        case TWI_I2C_MODE_COMBINED:
-               bfin_write_TWI_XMT_DATA8(iface->command);
-               bfin_write_TWI_INT_MASK(MCOMP | MERR | RCVSERV | XMTSERV);
+               write_XMT_DATA8(iface, iface->command);
+               write_INT_MASK(iface, MCOMP | MERR | RCVSERV | XMTSERV);
                SSYNC();
 
                if (iface->writeNum > 0)
-                       bfin_write_TWI_MASTER_CTL((iface->writeNum + 1) << 6);
+                       write_MASTER_CTL(iface, (iface->writeNum + 1) << 6);
                else
-                       bfin_write_TWI_MASTER_CTL(0x1 << 6);
+                       write_MASTER_CTL(iface, 0x1 << 6);
                /* Master enable */
-               bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() | MEN |
+               write_MASTER_CTL(iface, read_MASTER_CTL(iface) | MEN |
                        ((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ>100) ? FAST : 0));
                break;
        default:
-               bfin_write_TWI_MASTER_CTL(0);
+               write_MASTER_CTL(iface, 0);
                if (size != I2C_SMBUS_QUICK) {
                        /* Don't access xmit data register when this is a
                         * read operation.
                         */
                        if (iface->read_write != I2C_SMBUS_READ) {
                                if (iface->writeNum > 0) {
-                                       bfin_write_TWI_XMT_DATA8(*(iface->transPtr++));
+                                       write_XMT_DATA8(iface,
+                                               *(iface->transPtr++));
                                        if (iface->writeNum <= 255)
-                                               bfin_write_TWI_MASTER_CTL(iface->writeNum << 6);
+                                               write_MASTER_CTL(iface,
+                                                       iface->writeNum << 6);
                                        else {
-                                               bfin_write_TWI_MASTER_CTL(0xff << 6);
+                                               write_MASTER_CTL(iface,
+                                                       0xff << 6);
                                                iface->manual_stop = 1;
                                        }
                                        iface->writeNum--;
                                } else {
-                                       bfin_write_TWI_XMT_DATA8(iface->command);
-                                       bfin_write_TWI_MASTER_CTL(1 << 6);
+                                       write_XMT_DATA8(iface, iface->command);
+                                       write_MASTER_CTL(iface, 1 << 6);
                                }
                        } else {
                                if (iface->readNum > 0 && iface->readNum <= 255)
-                                       bfin_write_TWI_MASTER_CTL(iface->readNum << 6);
+                                       write_MASTER_CTL(iface,
+                                               iface->readNum << 6);
                                else if (iface->readNum > 255) {
-                                       bfin_write_TWI_MASTER_CTL(0xff << 6);
+                                       write_MASTER_CTL(iface, 0xff << 6);
                                        iface->manual_stop = 1;
                                } else {
                                        del_timer(&iface->timeout_timer);
@@ -516,13 +540,13 @@ int bfin_twi_smbus_xfer(struct i2c_adapter *adap, u16 addr,
                                }
                        }
                }
-               bfin_write_TWI_INT_MASK(MCOMP | MERR |
+               write_INT_MASK(iface, MCOMP | MERR |
                        ((iface->read_write == I2C_SMBUS_READ) ?
                        RCVSERV : XMTSERV));
                SSYNC();
 
                /* Master enable */
-               bfin_write_TWI_MASTER_CTL(bfin_read_TWI_MASTER_CTL() | MEN |
+               write_MASTER_CTL(iface, read_MASTER_CTL(iface) | MEN |
                        ((iface->read_write == I2C_SMBUS_READ) ? MDIR : 0) |
                        ((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ > 100) ? FAST : 0));
                break;
@@ -557,10 +581,10 @@ static struct i2c_algorithm bfin_twi_algorithm = {
 
 static int i2c_bfin_twi_suspend(struct platform_device *dev, pm_message_t state)
 {
-/*     struct bfin_twi_iface *iface = platform_get_drvdata(dev);*/
+       struct bfin_twi_iface *iface = platform_get_drvdata(dev);
 
        /* Disable TWI */
-       bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() & ~TWI_ENA);
+       write_CONTROL(iface, read_CONTROL(iface) & ~TWI_ENA);
        SSYNC();
 
        return 0;
@@ -568,24 +592,53 @@ static int i2c_bfin_twi_suspend(struct platform_device *dev, pm_message_t state)
 
 static int i2c_bfin_twi_resume(struct platform_device *dev)
 {
-/*     struct bfin_twi_iface *iface = platform_get_drvdata(dev);*/
+       struct bfin_twi_iface *iface = platform_get_drvdata(dev);
 
        /* Enable TWI */
-       bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() | TWI_ENA);
+       write_CONTROL(iface, read_CONTROL(iface) | TWI_ENA);
        SSYNC();
 
        return 0;
 }
 
-static int i2c_bfin_twi_probe(struct platform_device *dev)
+static int i2c_bfin_twi_probe(struct platform_device *pdev)
 {
-       struct bfin_twi_iface *iface = &twi_iface;
+       struct bfin_twi_iface *iface;
        struct i2c_adapter *p_adap;
+       struct resource *res;
        int rc;
 
+       iface = kzalloc(sizeof(struct bfin_twi_iface), GFP_KERNEL);
+       if (!iface) {
+               dev_err(&pdev->dev, "Cannot allocate memory\n");
+               rc = -ENOMEM;
+               goto out_error_nomem;
+       }
+
        spin_lock_init(&(iface->lock));
        init_completion(&(iface->complete));
-       iface->irq = IRQ_TWI;
+
+       /* Find and map our resources */
+       res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+       if (res == NULL) {
+               dev_err(&pdev->dev, "Cannot get IORESOURCE_MEM\n");
+               rc = -ENOENT;
+               goto out_error_get_res;
+       }
+
+       iface->regs_base = ioremap(res->start, res->end - res->start + 1);
+       if (iface->regs_base == NULL) {
+               dev_err(&pdev->dev, "Cannot map IO\n");
+               rc = -ENXIO;
+               goto out_error_ioremap;
+       }
+
+       iface->irq = platform_get_irq(pdev, 0);
+       if (iface->irq < 0) {
+               dev_err(&pdev->dev, "No IRQ specified\n");
+               rc = -ENOENT;
+               goto out_error_no_irq;
+       }
 
        init_timer(&(iface->timeout_timer));
        iface->timeout_timer.function = bfin_twi_timeout;
@@ -593,39 +646,55 @@ static int i2c_bfin_twi_probe(struct platform_device *dev)
 
        p_adap = &iface->adap;
        p_adap->id = I2C_HW_BLACKFIN;
-       p_adap->nr = dev->id;
-       strlcpy(p_adap->name, dev->name, sizeof(p_adap->name));
+       p_adap->nr = pdev->id;
+       strlcpy(p_adap->name, pdev->name, sizeof(p_adap->name));
        p_adap->algo = &bfin_twi_algorithm;
        p_adap->algo_data = iface;
        p_adap->class = I2C_CLASS_ALL;
-       p_adap->dev.parent = &dev->dev;
+       p_adap->dev.parent = &pdev->dev;
 
        rc = request_irq(iface->irq, bfin_twi_interrupt_entry,
-               IRQF_DISABLED, dev->name, iface);
+               IRQF_DISABLED, pdev->name, iface);
        if (rc) {
-               dev_err(&(p_adap->dev), "i2c-bfin-twi: can't get IRQ %d !\n",
-                       iface->irq);
-               return -ENODEV;
+               dev_err(&pdev->dev, "Can't get IRQ %d !\n", iface->irq);
+               rc = -ENODEV;
+               goto out_error_req_irq;
        }
 
        /* Set TWI internal clock as 10MHz */
-       bfin_write_TWI_CONTROL(((get_sclk() / 1024 / 1024 + 5) / 10) & 0x7F);
+       write_CONTROL(iface, ((get_sclk() / 1024 / 1024 + 5) / 10) & 0x7F);
 
        /* Set Twi interface clock as specified */
-       bfin_write_TWI_CLKDIV((( 5*1024 / CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ )
-                       << 8) | (( 5*1024 / CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ )
+       write_CLKDIV(iface, ((5*1024 / CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ)
+                       << 8) | ((5*1024 / CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ)
                        & 0xFF));
 
        /* Enable TWI */
-       bfin_write_TWI_CONTROL(bfin_read_TWI_CONTROL() | TWI_ENA);
+       write_CONTROL(iface, read_CONTROL(iface) | TWI_ENA);
        SSYNC();
 
        rc = i2c_add_numbered_adapter(p_adap);
-       if (rc < 0)
-               free_irq(iface->irq, iface);
-       else
-               platform_set_drvdata(dev, iface);
+       if (rc < 0) {
+               dev_err(&pdev->dev, "Can't add i2c adapter!\n");
+               goto out_error_add_adapter;
+       }
+
+       platform_set_drvdata(pdev, iface);
 
+       dev_info(&pdev->dev, "Blackfin I2C TWI controller, regs_base@%p\n",
+               iface->regs_base);
+
+       return 0;
+
+out_error_add_adapter:
+       free_irq(iface->irq, iface);
+out_error_req_irq:
+out_error_no_irq:
+       iounmap(iface->regs_base);
+out_error_ioremap:
+out_error_get_res:
+       kfree(iface);
+out_error_nomem:
        return rc;
 }
 
@@ -637,6 +706,8 @@ static int i2c_bfin_twi_remove(struct platform_device *pdev)
 
        i2c_del_adapter(&(iface->adap));
        free_irq(iface->irq, iface);
+       iounmap(iface->regs_base);
+       kfree(iface);
 
        return 0;
 }
@@ -654,8 +725,6 @@ static struct platform_driver i2c_bfin_twi_driver = {
 
 static int __init i2c_bfin_twi_init(void)
 {
-       pr_info("I2C: Blackfin I2C TWI driver\n");
-
        return platform_driver_register(&i2c_bfin_twi_driver);
 }