Patchwork [U-Boot,4/4,V5] I2C: mxc_i2c rework

login
register
mail settings
Submitter Marek Vasut
Date Sept. 22, 2011, 7:22 p.m.
Message ID <1316719332-31185-1-git-send-email-marek.vasut@gmail.com>
Download mbox | patch
Permalink /patch/116003/
State Accepted
Commit db84140bb7cfbe60d200cca72f79c37ec7ae87d6
Delegated to: Stefano Babic
Headers show

Comments

Marek Vasut - Sept. 22, 2011, 7:22 p.m.
Rewrite the mxc_i2c driver.
 * This version is much closer to Linux implementation.
 * Fixes IPG_PERCLK being incorrectly used as clock source
 * Fixes behaviour of the driver on iMX51
 * Clean up coding style a bit ;-)

Signed-off-by: Marek Vasut <marek.vasut@gmail.com>
Cc: Stefano Babic <sbabic@denx.de>
Cc: Heiko Schocher <hs@denx.de>
Cc: Jason Hui <jason.hui@linaro.org>
---
 drivers/i2c/mxc_i2c.c |  422 +++++++++++++++++++++++++++++++++----------------
 1 files changed, 289 insertions(+), 133 deletions(-)

V2: Use PERCLK as a source.
V3: Remove forgotten unused variables.
V4: Add missing Cc field to commit message.
V5: Correct TX_NO_AK bit handling.
Jason Liu - Sept. 23, 2011, 3:32 a.m.
Hi, Marek,

On Fri, Sep 23, 2011 at 3:22 AM, Marek Vasut <marek.vasut@gmail.com> wrote:
> Rewrite the mxc_i2c driver.
>  * This version is much closer to Linux implementation.
>  * Fixes IPG_PERCLK being incorrectly used as clock source
>  * Fixes behaviour of the driver on iMX51
>  * Clean up coding style a bit ;-)
>
> Signed-off-by: Marek Vasut <marek.vasut@gmail.com>
> Cc: Stefano Babic <sbabic@denx.de>
> Cc: Heiko Schocher <hs@denx.de>
> Cc: Jason Hui <jason.hui@linaro.org>
> ---
>  drivers/i2c/mxc_i2c.c |  422 +++++++++++++++++++++++++++++++++----------------
>  1 files changed, 289 insertions(+), 133 deletions(-)
>
> V2: Use PERCLK as a source.
> V3: Remove forgotten unused variables.
> V4: Add missing Cc field to commit message.
> V5: Correct TX_NO_AK bit handling.
>

Test on i.MX53evk, it woks now.

Acked-by: Jason Liu <jason.hui@linro.org>
Tested-by: Jason Liu <jason.hui@linro.org>


BR,
Jason Liu

>
Fabio Estevam - Sept. 23, 2011, 4:31 a.m.
On Fri, Sep 23, 2011 at 12:32 AM, Jason Hui <jason.hui@linaro.org> wrote:
> Test on i.MX53evk, it woks now.
>
> Acked-by: Jason Liu <jason.hui@linro.org>
> Tested-by: Jason Liu <jason.hui@linro.org>

I guess you meant linaro.org ;-)
Marek Vasut - Sept. 23, 2011, 6:21 a.m.
On Friday, September 23, 2011 05:32:13 AM Jason Hui wrote:
> Hi, Marek,
> 
> On Fri, Sep 23, 2011 at 3:22 AM, Marek Vasut <marek.vasut@gmail.com> wrote:
> > Rewrite the mxc_i2c driver.
> >  * This version is much closer to Linux implementation.
> >  * Fixes IPG_PERCLK being incorrectly used as clock source
> >  * Fixes behaviour of the driver on iMX51
> >  * Clean up coding style a bit ;-)
> > 
> > Signed-off-by: Marek Vasut <marek.vasut@gmail.com>
> > Cc: Stefano Babic <sbabic@denx.de>
> > Cc: Heiko Schocher <hs@denx.de>
> > Cc: Jason Hui <jason.hui@linaro.org>
> > ---
> >  drivers/i2c/mxc_i2c.c |  422
> > +++++++++++++++++++++++++++++++++---------------- 1 files changed, 289
> > insertions(+), 133 deletions(-)
> > 
> > V2: Use PERCLK as a source.
> > V3: Remove forgotten unused variables.
> > V4: Add missing Cc field to commit message.
> > V5: Correct TX_NO_AK bit handling.
> 
> Test on i.MX53evk, it woks now.
> 
> Acked-by: Jason Liu <jason.hui@linro.org>
> Tested-by: Jason Liu <jason.hui@linro.org>

Thanks.

When can I expect you commiting the DIALOG PMIC of doom code ?

Cheers
> 
> 
> BR,
> Jason Liu
Stefano Babic - Sept. 23, 2011, 7:46 a.m.
On 09/23/2011 05:32 AM, Jason Hui wrote:
> Hi, Marek,
> 
> On Fri, Sep 23, 2011 at 3:22 AM, Marek Vasut <marek.vasut@gmail.com> wrote:
>> Rewrite the mxc_i2c driver.
>>  * This version is much closer to Linux implementation.
>>  * Fixes IPG_PERCLK being incorrectly used as clock source
>>  * Fixes behaviour of the driver on iMX51
>>  * Clean up coding style a bit ;-)
>>
>> Signed-off-by: Marek Vasut <marek.vasut@gmail.com>
>> Cc: Stefano Babic <sbabic@denx.de>
>> Cc: Heiko Schocher <hs@denx.de>
>> Cc: Jason Hui <jason.hui@linaro.org>
>> ---
>>  drivers/i2c/mxc_i2c.c |  422 +++++++++++++++++++++++++++++++++----------------
>>  1 files changed, 289 insertions(+), 133 deletions(-)
>>
>> V2: Use PERCLK as a source.
>> V3: Remove forgotten unused variables.
>> V4: Add missing Cc field to commit message.
>> V5: Correct TX_NO_AK bit handling.
>>
> 
> Test on i.MX53evk, it woks now.
> 
> Acked-by: Jason Liu <jason.hui@linro.org>
> Tested-by: Jason Liu <jason.hui@linro.org>

Thanks everybody for the good work !

I will merge now this series into u-boot-imx. I do not see any open
issus (please correct me if I am wrong !).

Best regards,
Stefano Babic
Stefano Babic - Sept. 23, 2011, 1:20 p.m.
On 09/22/2011 09:22 PM, Marek Vasut wrote:
> Rewrite the mxc_i2c driver.
>  * This version is much closer to Linux implementation.
>  * Fixes IPG_PERCLK being incorrectly used as clock source
>  * Fixes behaviour of the driver on iMX51
>  * Clean up coding style a bit ;-)
> 
> Signed-off-by: Marek Vasut <marek.vasut@gmail.com>
> Cc: Stefano Babic <sbabic@denx.de>
> Cc: Heiko Schocher <hs@denx.de>
> Cc: Jason Hui <jason.hui@linaro.org>
> ---

Applied to-uboot-imx, next branch, thanks.

Best regards,
Stefano Babic
Heiko Schocher - Sept. 26, 2011, 6:30 a.m.
Hello Stefano,

Stefano Babic wrote:
> On 09/23/2011 05:32 AM, Jason Hui wrote:
>> Hi, Marek,
>>
>> On Fri, Sep 23, 2011 at 3:22 AM, Marek Vasut <marek.vasut@gmail.com> wrote:
>>> Rewrite the mxc_i2c driver.
>>>  * This version is much closer to Linux implementation.
>>>  * Fixes IPG_PERCLK being incorrectly used as clock source
>>>  * Fixes behaviour of the driver on iMX51
>>>  * Clean up coding style a bit ;-)
>>>
>>> Signed-off-by: Marek Vasut <marek.vasut@gmail.com>
>>> Cc: Stefano Babic <sbabic@denx.de>
>>> Cc: Heiko Schocher <hs@denx.de>
>>> Cc: Jason Hui <jason.hui@linaro.org>
>>> ---
>>>  drivers/i2c/mxc_i2c.c |  422 +++++++++++++++++++++++++++++++++----------------
>>>  1 files changed, 289 insertions(+), 133 deletions(-)
>>>
>>> V2: Use PERCLK as a source.
>>> V3: Remove forgotten unused variables.
>>> V4: Add missing Cc field to commit message.
>>> V5: Correct TX_NO_AK bit handling.
>>>
>> Test on i.MX53evk, it woks now.
>>
>> Acked-by: Jason Liu <jason.hui@linro.org>
>> Tested-by: Jason Liu <jason.hui@linro.org>
> 
> Thanks everybody for the good work !

Yep! Thanks to Marek and Jason.

> I will merge now this series into u-boot-imx. I do not see any open
> issus (please correct me if I am wrong !).

Acked-by: Heiko Schocher <hs@denx.de>

bye,
Heiko

Patch

diff --git a/drivers/i2c/mxc_i2c.c b/drivers/i2c/mxc_i2c.c
index ebde3c5..a805bf6 100644
--- a/drivers/i2c/mxc_i2c.c
+++ b/drivers/i2c/mxc_i2c.c
@@ -1,7 +1,15 @@ 
 /*
- * i2c driver for Freescale mx31
+ * i2c driver for Freescale i.MX series
  *
  * (c) 2007 Pengutronix, Sascha Hauer <s.hauer@pengutronix.de>
+ * (c) 2011 Marek Vasut <marek.vasut@gmail.com>
+ *
+ * Based on i2c-imx.c from linux kernel:
+ *  Copyright (C) 2005 Torsten Koschorrek <koschorrek at synertronixx.de>
+ *  Copyright (C) 2005 Matthias Blaschke <blaschke at synertronixx.de>
+ *  Copyright (C) 2007 RightHand Technologies, Inc.
+ *  Copyright (C) 2008 Darius Augulis <darius.augulis at teltonika.lt>
+ *
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -30,11 +38,13 @@ 
 #include <asm/arch/clock.h>
 #include <asm/arch/imx-regs.h>
 
-#define IADR	0x00
-#define IFDR	0x04
-#define I2CR	0x08
-#define I2SR	0x0c
-#define I2DR	0x10
+struct mxc_i2c_regs {
+	uint32_t	iadr;
+	uint32_t	ifdr;
+	uint32_t	i2cr;
+	uint32_t	i2sr;
+	uint32_t	i2dr;
+};
 
 #define I2CR_IEN	(1 << 7)
 #define I2CR_IIEN	(1 << 6)
@@ -68,215 +78,361 @@ 
 #endif
 
 #define I2C_MAX_TIMEOUT		10000
-#define I2C_MAX_RETRIES		3
 
-static u16 div[] = { 30, 32, 36, 42, 48, 52, 60, 72, 80, 88, 104, 128, 144,
-	             160, 192, 240, 288, 320, 384, 480, 576, 640, 768, 960,
-	             1152, 1280, 1536, 1920, 2304, 2560, 3072, 3840};
+static u16 i2c_clk_div[50][2] = {
+	{ 22,	0x20 }, { 24,	0x21 }, { 26,	0x22 }, { 28,	0x23 },
+	{ 30,	0x00 }, { 32,	0x24 }, { 36,	0x25 }, { 40,	0x26 },
+	{ 42,	0x03 }, { 44,	0x27 }, { 48,	0x28 }, { 52,	0x05 },
+	{ 56,	0x29 }, { 60,	0x06 }, { 64,	0x2A }, { 72,	0x2B },
+	{ 80,	0x2C }, { 88,	0x09 }, { 96,	0x2D }, { 104,	0x0A },
+	{ 112,	0x2E }, { 128,	0x2F }, { 144,	0x0C }, { 160,	0x30 },
+	{ 192,	0x31 }, { 224,	0x32 }, { 240,	0x0F }, { 256,	0x33 },
+	{ 288,	0x10 }, { 320,	0x34 }, { 384,	0x35 }, { 448,	0x36 },
+	{ 480,	0x13 }, { 512,	0x37 }, { 576,	0x14 }, { 640,	0x38 },
+	{ 768,	0x39 }, { 896,	0x3A }, { 960,	0x17 }, { 1024,	0x3B },
+	{ 1152,	0x18 }, { 1280,	0x3C }, { 1536,	0x3D }, { 1792,	0x3E },
+	{ 1920,	0x1B }, { 2048,	0x3F }, { 2304,	0x1C }, { 2560,	0x1D },
+	{ 3072,	0x1E }, { 3840,	0x1F }
+};
+
+static u8 clk_idx;
 
-static inline void i2c_reset(void)
-{
-	writew(0, I2C_BASE + I2CR);	/* Reset module */
-	writew(0, I2C_BASE + I2SR);
-	writew(I2CR_IEN, I2C_BASE + I2CR);
-}
-
-void i2c_init(int speed, int unused)
+/*
+ * Calculate and set proper clock divider
+ */
+static void i2c_imx_set_clk(unsigned int rate)
 {
-	int freq;
+	struct mxc_i2c_regs *i2c_regs = (struct mxc_i2c_regs *)I2C_BASE;
+	unsigned int i2c_clk_rate;
+	unsigned int div;
 	int i;
 
 #if defined(CONFIG_MX31)
 	struct clock_control_regs *sc_regs =
 		(struct clock_control_regs *)CCM_BASE;
+
 	/* start the required I2C clock */
 	writel(readl(&sc_regs->cgr0) | (3 << I2C_CLK_OFFSET),
 		&sc_regs->cgr0);
 #endif
-	freq = mxc_get_clock(MXC_IPG_PERCLK);
 
-	for (i = 0; i < 0x1f; i++)
-		if (freq / div[i] <= speed)
-			break;
+	/* Divider value calculation */
+	i2c_clk_rate = mxc_get_clock(MXC_IPG_PERCLK);
+	div = (i2c_clk_rate + rate - 1) / rate;
+	if (div < i2c_clk_div[0][0])
+		i = 0;
+	else if (div > i2c_clk_div[ARRAY_SIZE(i2c_clk_div) - 1][0])
+		i = ARRAY_SIZE(i2c_clk_div) - 1;
+	else
+		for (i = 0; i2c_clk_div[i][0] < div; i++)
+			;
+
+	/* Store divider value */
+	clk_idx = i2c_clk_div[i][1];
+	writeb(clk_idx, &i2c_regs->ifdr);
+}
 
-	debug("%s: speed: %d\n", __func__, speed);
+/*
+ * Reset I2C Controller
+ */
+void i2c_reset(void)
+{
+	struct mxc_i2c_regs *i2c_regs = (struct mxc_i2c_regs *)I2C_BASE;
+
+	writeb(0, &i2c_regs->i2cr);	/* Reset module */
+	writeb(0, &i2c_regs->i2sr);
+}
 
-	writew(i, I2C_BASE + IFDR);
+/*
+ * Init I2C Bus
+ */
+void i2c_init(int speed, int unused)
+{
+	i2c_imx_set_clk(speed);
 	i2c_reset();
 }
 
-static int wait_idle(void)
+/*
+ * Wait for bus to be busy (or free if for_busy = 0)
+ *
+ * for_busy = 1: Wait for IBB to be asserted
+ * for_busy = 0: Wait for IBB to be de-asserted
+ */
+int i2c_imx_bus_busy(int for_busy)
 {
+	struct mxc_i2c_regs *i2c_regs = (struct mxc_i2c_regs *)I2C_BASE;
+	unsigned int temp;
+
 	int timeout = I2C_MAX_TIMEOUT;
 
-	while ((readw(I2C_BASE + I2SR) & I2SR_IBB) && --timeout) {
-		writew(0, I2C_BASE + I2SR);
+	while (timeout--) {
+		temp = readb(&i2c_regs->i2sr);
+
+		if (for_busy && (temp & I2SR_IBB))
+			return 0;
+		if (!for_busy && !(temp & I2SR_IBB))
+			return 0;
+
 		udelay(1);
 	}
-	return timeout ? timeout : (!(readw(I2C_BASE + I2SR) & I2SR_IBB));
+
+	return 1;
 }
 
-static int wait_busy(void)
+/*
+ * Wait for transaction to complete
+ */
+int i2c_imx_trx_complete(void)
 {
+	struct mxc_i2c_regs *i2c_regs = (struct mxc_i2c_regs *)I2C_BASE;
 	int timeout = I2C_MAX_TIMEOUT;
 
-	while (!(readw(I2C_BASE + I2SR) & I2SR_IBB) && --timeout)
+	while (timeout--) {
+		if (readb(&i2c_regs->i2sr) & I2SR_IIF) {
+			writeb(0, &i2c_regs->i2sr);
+			return 0;
+		}
+
 		udelay(1);
-	writew(0, I2C_BASE + I2SR); /* clear interrupt */
+	}
 
-	return timeout;
+	return 1;
 }
 
-static int wait_complete(void)
+/*
+ * Check if the transaction was ACKed
+ */
+int i2c_imx_acked(void)
 {
-	int timeout = I2C_MAX_TIMEOUT;
+	struct mxc_i2c_regs *i2c_regs = (struct mxc_i2c_regs *)I2C_BASE;
 
-	while ((!(readw(I2C_BASE + I2SR) & I2SR_ICF)) && (--timeout)) {
-		writew(0, I2C_BASE + I2SR);
-		udelay(1);
-	}
-	udelay(200);
+	return readb(&i2c_regs->i2sr) & I2SR_RX_NO_AK;
+}
 
-	writew(0, I2C_BASE + I2SR);	/* clear interrupt */
+/*
+ * Start the controller
+ */
+int i2c_imx_start(void)
+{
+	struct mxc_i2c_regs *i2c_regs = (struct mxc_i2c_regs *)I2C_BASE;
+	unsigned int temp = 0;
+	int result;
 
-	return timeout;
-}
+	writeb(clk_idx, &i2c_regs->ifdr);
 
+	/* Enable I2C controller */
+	writeb(0, &i2c_regs->i2sr);
+	writeb(I2CR_IEN, &i2c_regs->i2cr);
 
-static int tx_byte(u8 byte)
-{
-	writew(byte, I2C_BASE + I2DR);
+	/* Wait controller to be stable */
+	udelay(50);
+
+	/* Start I2C transaction */
+	temp = readb(&i2c_regs->i2cr);
+	temp |= I2CR_MSTA;
+	writeb(temp, &i2c_regs->i2cr);
+
+	result = i2c_imx_bus_busy(1);
+	if (result)
+		return result;
+
+	temp |= I2CR_MTX | I2CR_TX_NO_AK;
+	writeb(temp, &i2c_regs->i2cr);
 
-	if (!wait_complete() || readw(I2C_BASE + I2SR) & I2SR_RX_NO_AK)
-		return -1;
 	return 0;
 }
 
-static int rx_byte(int last)
+/*
+ * Stop the controller
+ */
+void i2c_imx_stop(void)
 {
-	if (!wait_complete())
-		return -1;
+	struct mxc_i2c_regs *i2c_regs = (struct mxc_i2c_regs *)I2C_BASE;
+	unsigned int temp = 0;
+
+	/* Stop I2C transaction */
+	temp = readb(&i2c_regs->i2cr);
+	temp |= ~(I2CR_MSTA | I2CR_MTX);
+	writeb(temp, &i2c_regs->i2cr);
 
-	if (last)
-		writew(I2CR_IEN, I2C_BASE + I2CR);
+	i2c_imx_bus_busy(0);
 
-	return readw(I2C_BASE + I2DR);
+	/* Disable I2C controller */
+	writeb(0, &i2c_regs->i2cr);
 }
 
-int i2c_probe(uchar chip)
+/*
+ * Set chip address and access mode
+ *
+ * read = 1: READ access
+ * read = 0: WRITE access
+ */
+int i2c_imx_set_chip_addr(uchar chip, int read)
 {
+	struct mxc_i2c_regs *i2c_regs = (struct mxc_i2c_regs *)I2C_BASE;
 	int ret;
 
-	writew(0, I2C_BASE + I2CR); /* Reset module */
-	writew(I2CR_IEN, I2C_BASE + I2CR);
+	writeb((chip << 1) | read, &i2c_regs->i2dr);
+
+	ret = i2c_imx_trx_complete();
+	if (ret)
+		return ret;
 
-	writew(I2CR_IEN |  I2CR_MSTA | I2CR_MTX, I2C_BASE + I2CR);
-	ret = tx_byte(chip << 1);
-	writew(I2CR_IEN | I2CR_MTX, I2C_BASE + I2CR);
+	ret = i2c_imx_acked();
+	if (ret)
+		return ret;
 
 	return ret;
 }
 
-static int i2c_addr(uchar chip, uint addr, int alen)
+/*
+ * Write register address
+ */
+int i2c_imx_set_reg_addr(uint addr, int alen)
 {
-	int i, retry = 0;
-	for (retry = 0; retry < 3; retry++) {
-		if (wait_idle())
+	struct mxc_i2c_regs *i2c_regs = (struct mxc_i2c_regs *)I2C_BASE;
+	int ret;
+	int i;
+
+	for (i = 0; i < (8 * alen); i += 8) {
+		writeb((addr >> i) & 0xff, &i2c_regs->i2dr);
+
+		ret = i2c_imx_trx_complete();
+		if (ret)
 			break;
-		i2c_reset();
-		for (i = 0; i < I2C_MAX_TIMEOUT; i++)
-			udelay(1);
-	}
-	if (retry >= I2C_MAX_RETRIES) {
-		debug("%s:bus is busy(%x)\n",
-		       __func__, readw(I2C_BASE + I2SR));
-		return -1;
-	}
-	writew(I2CR_IEN | I2CR_MSTA | I2CR_MTX, I2C_BASE + I2CR);
 
-	if (!wait_busy()) {
-		debug("%s:trigger start fail(%x)\n",
-		       __func__, readw(I2C_BASE + I2SR));
-		return -1;
+		ret = i2c_imx_acked();
+		if (ret)
+			break;
 	}
 
-	if (tx_byte(chip << 1) || (readw(I2C_BASE + I2SR) & I2SR_RX_NO_AK)) {
-		debug("%s:chip address cycle fail(%x)\n",
-		       __func__, readw(I2C_BASE + I2SR));
-		return -1;
-	}
-	while (alen--)
-		if (tx_byte((addr >> (alen * 8)) & 0xff) ||
-		    (readw(I2C_BASE + I2SR) & I2SR_RX_NO_AK)) {
-			debug("%s:device address cycle fail(%x)\n",
-			       __func__, readw(I2C_BASE + I2SR));
-			return -1;
-		}
-	return 0;
+	return ret;
 }
 
-int i2c_read(uchar chip, uint addr, int alen, uchar *buf, int len)
+/*
+ * Try if a chip add given address responds (probe the chip)
+ */
+int i2c_probe(uchar chip)
 {
-	int timeout = I2C_MAX_TIMEOUT;
 	int ret;
 
-	debug("%s chip: 0x%02x addr: 0x%04x alen: %d len: %d\n",
-		__func__, chip, addr, alen, len);
+	ret = i2c_imx_start();
+	if (ret)
+		return ret;
 
-	if (i2c_addr(chip, addr, alen)) {
-		printf("i2c_addr failed\n");
-		return -1;
-	}
+	ret = i2c_imx_set_chip_addr(chip, 0);
+	if (ret)
+		return ret;
 
-	writew(I2CR_IEN | I2CR_MSTA | I2CR_MTX | I2CR_RSTA, I2C_BASE + I2CR);
+	i2c_imx_stop();
 
-	if (tx_byte(chip << 1 | 1))
-		return -1;
+	return ret;
+}
 
-	writew(I2CR_IEN | I2CR_MSTA |
-		((len == 1) ? I2CR_TX_NO_AK : 0),
-		I2C_BASE + I2CR);
+/*
+ * Read data from I2C device
+ */
+int i2c_read(uchar chip, uint addr, int alen, uchar *buf, int len)
+{
+	struct mxc_i2c_regs *i2c_regs = (struct mxc_i2c_regs *)I2C_BASE;
+	int ret;
+	unsigned int temp;
+	int i;
 
-	ret = readw(I2C_BASE + I2DR);
+	ret = i2c_imx_start();
+	if (ret)
+		return ret;
+
+	/* write slave address */
+	ret = i2c_imx_set_chip_addr(chip, 0);
+	if (ret)
+		return ret;
+
+	ret = i2c_imx_set_reg_addr(addr, alen);
+	if (ret)
+		return ret;
+
+	temp = readb(&i2c_regs->i2cr);
+	temp |= I2CR_RSTA;
+	writeb(temp, &i2c_regs->i2cr);
+
+	ret = i2c_imx_set_chip_addr(chip, 1);
+	if (ret)
+		return ret;
+
+	/* setup bus to read data */
+	temp = readb(&i2c_regs->i2cr);
+	temp &= ~(I2CR_MTX | I2CR_TX_NO_AK);
+	if (len == 1)
+		temp |= I2CR_TX_NO_AK;
+	writeb(temp, &i2c_regs->i2cr);
+	readb(&i2c_regs->i2dr);
+
+	/* read data */
+	for (i = 0; i < len; i++) {
+		ret = i2c_imx_trx_complete();
+		if (ret)
+			return ret;
+
+		/*
+		 * It must generate STOP before read I2DR to prevent
+		 * controller from generating another clock cycle
+		 */
+		if (i == (len - 1)) {
+			temp = readb(&i2c_regs->i2cr);
+			temp &= ~(I2CR_MSTA | I2CR_MTX);
+			writeb(temp, &i2c_regs->i2cr);
+			i2c_imx_bus_busy(0);
+		} else if (i == (len - 2)) {
+			temp = readb(&i2c_regs->i2cr);
+			temp |= I2CR_TX_NO_AK;
+			writeb(temp, &i2c_regs->i2cr);
+		}
 
-	while (len--) {
-		ret = rx_byte(len == 0);
-		if (ret  < 0)
-			return -1;
-		*buf++ = ret;
-		if (len <= 1)
-			writew(I2CR_IEN | I2CR_MSTA |
-				I2CR_TX_NO_AK,
-				I2C_BASE + I2CR);
+		buf[i] = readb(&i2c_regs->i2dr);
 	}
 
-	writew(I2CR_IEN, I2C_BASE + I2CR);
-
-	while (readw(I2C_BASE + I2SR) & I2SR_IBB && --timeout)
-		udelay(1);
+	i2c_imx_stop();
 
-	return 0;
+	return ret;
 }
 
+/*
+ * Write data to I2C device
+ */
 int i2c_write(uchar chip, uint addr, int alen, uchar *buf, int len)
 {
-	int timeout = I2C_MAX_TIMEOUT;
-	debug("%s chip: 0x%02x addr: 0x%04x alen: %d len: %d\n",
-		__func__, chip, addr, alen, len);
+	struct mxc_i2c_regs *i2c_regs = (struct mxc_i2c_regs *)I2C_BASE;
+	int ret;
+	int i;
 
-	if (i2c_addr(chip, addr, alen))
-		return -1;
+	ret = i2c_imx_start();
+	if (ret)
+		return ret;
 
-	while (len--)
-		if (tx_byte(*buf++))
-			return -1;
+	/* write slave address */
+	ret = i2c_imx_set_chip_addr(chip, 0);
+	if (ret)
+		return ret;
 
-	writew(I2CR_IEN, I2C_BASE + I2CR);
+	ret = i2c_imx_set_reg_addr(addr, alen);
+	if (ret)
+		return ret;
 
-	while (readw(I2C_BASE + I2SR) & I2SR_IBB && --timeout)
-		udelay(1);
+	for (i = 0; i < len; i++) {
+		writeb(buf[i], &i2c_regs->i2dr);
 
-	return 0;
-}
+		ret = i2c_imx_trx_complete();
+		if (ret)
+			return ret;
+
+		ret = i2c_imx_acked();
+		if (ret)
+			return ret;
+	}
 
+	i2c_imx_stop();
+
+	return ret;
+}
 #endif /* CONFIG_HARD_I2C */