Patchwork [U-Boot] I2C: mxc_i2c rework

login
register
mail settings
Submitter Marek Vasut
Date July 13, 2011, 9:53 a.m.
Message ID <1310550794-27795-1-git-send-email-marek.vasut@gmail.com>
Download mbox | patch
Permalink /patch/104496/
State Changes Requested
Headers show

Comments

Marek Vasut - July 13, 2011, 9:53 a.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>
---
 drivers/i2c/mxc_i2c.c |  391 +++++++++++++++++++++++++++++++++----------------
 1 files changed, 262 insertions(+), 129 deletions(-)
Stefano Babic - July 13, 2011, 1:56 p.m.
On 07/13/2011 11:53 AM, 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>
> ---

Hi Marek,

I have added Heiko in CC. He is the Maintainer for I2C.

>  #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 }
> +};

You have added an array with fixed values as indicated in the
Freescale's manual (Table 26-7 for i.MX31, Table 40-7 for MX51, Table
41-12 for MX53). What about to add also some comments about these changes ?


> -void i2c_init(int speed, int unused)
> +/*
> + * Calculate and set proper clock divider
> + *
> + * FIXME: remove #ifdefs !
> + */

Agree. I have prepared a patch to get rid of this mx31_ specialties. I
will post soon. Then we can use mxc_get_clock(MXC_IPG_CLK) for all i.MX
processors.


> +	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 */
> +	writeb(div, I2C_BASE + IFDR);
> +	clk_idx = div;

It seems to me ok - you replaced a computed value, that does not obtain
exactly the value indicated in the manual, with the closest value of the
table.

> +int i2c_imx_bus_busy(int for_busy)
>  {
> +	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_BASE + 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;
>  }

It is not clear to me why you add a way to go out from the function. If
it is busy, should we not wait at least until the timeout variable
becomes zero ?


>  
> -static int wait_busy(void)
> +/*
> + * Wait for transaction to complete
> + */
> +int i2c_imx_trx_complete(void)
>  {
>  	int timeout = I2C_MAX_TIMEOUT;
>  
> -	while (!(readw(I2C_BASE + I2SR) & I2SR_IBB) && --timeout)
> +	while (timeout--) {
> +		if (readb(I2C_BASE + I2SR) & I2SR_IIF) {

If we wait for completion, should we not check the ICF bit instead of
IIF, as done before your patch ?

> +/*
> + * Start the controller
> + */
> +int i2c_imx_start(void)
> +{
> +	unsigned int temp = 0;
> +	int result;
>  
> -	writew(0, I2C_BASE + I2SR);	/* clear interrupt */
> +	writeb(clk_idx, I2C_BASE + IFDR);

Well, as you talk about cleaning up the code, what about to replace the
direct access to the registers with a C structure, as part of your clean
up ?


> +/*
> + * 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())
> +	int ret;
> +	int i;
> +

mmmhh...it seems to me you change completely the logic here. Heiko, waht
do you think about ?

Best regards,
Stefano Babic
Marek Vasut - July 13, 2011, 5:12 p.m.
On Wednesday, July 13, 2011 03:56:45 PM Stefano Babic wrote:
> On 07/13/2011 11:53 AM, 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>
> > ---
> 
> Hi Marek,
> 
> I have added Heiko in CC. He is the Maintainer for I2C.
> 
> >  #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 }
> > +};
> 
> You have added an array with fixed values as indicated in the
> Freescale's manual (Table 26-7 for i.MX31, Table 40-7 for MX51, Table
> 41-12 for MX53). What about to add also some comments about these changes ?

I stole this from linux kernel, but sure ... as this is nearly a complete 
rewrite, I didn't describe all the changes.
> 
> > -void i2c_init(int speed, int unused)
> > +/*
> > + * Calculate and set proper clock divider
> > + *
> > + * FIXME: remove #ifdefs !
> > + */
> 
> Agree. I have prepared a patch to get rid of this mx31_ specialties. I
> will post soon. Then we can use mxc_get_clock(MXC_IPG_CLK) for all i.MX
> processors.

Yes, thanks. I saw the patch you posted already.
> 
> > +	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 */
> > +	writeb(div, I2C_BASE + IFDR);
> > +	clk_idx = div;
> 
> It seems to me ok - you replaced a computed value, that does not obtain
> exactly the value indicated in the manual, with the closest value of the
> table.

This thing is from linux kernel, nearly exact copy.
> 
> > +int i2c_imx_bus_busy(int for_busy)
> > 
> >  {
> > 
> > +	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_BASE + 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;
> > 
> >  }
> 
> It is not clear to me why you add a way to go out from the function. If
> it is busy, should we not wait at least until the timeout variable
> becomes zero ?

You are waiting until it times out ... you can use thing to either wait for the 
bus to become busy or to become free, check the description of this function :)

> 
> > -static int wait_busy(void)
> > +/*
> > + * Wait for transaction to complete
> > + */
> > +int i2c_imx_trx_complete(void)
> > 
> >  {
> >  
> >  	int timeout = I2C_MAX_TIMEOUT;
> > 
> > -	while (!(readw(I2C_BASE + I2SR) & I2SR_IBB) && --timeout)
> > +	while (timeout--) {
> > +		if (readb(I2C_BASE + I2SR) & I2SR_IIF) {
> 
> If we wait for completion, should we not check the ICF bit instead of
> IIF, as done before your patch ?

That's how this is done in linux kernel, and TBH the only possible occurance of 
IIF is exactly when ICF is set too (in our case at least). IIRC when I last 
tried this, I had some trouble with ICF, but this is a valid point.

I'll investigate further here.

> 
> > +/*
> > + * Start the controller
> > + */
> > +int i2c_imx_start(void)
> > +{
> > +	unsigned int temp = 0;
> > +	int result;
> > 
> > -	writew(0, I2C_BASE + I2SR);	/* clear interrupt */
> > +	writeb(clk_idx, I2C_BASE + IFDR);
> 
> Well, as you talk about cleaning up the code, what about to replace the
> direct access to the registers with a C structure, as part of your clean
> up ?

Very good point!
> 
> > +/*
> > + * 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())
> > +	int ret;
> > +	int i;
> > +
> 
> mmmhh...it seems to me you change completely the logic here. Heiko, waht
> do you think about ?
> 
> Best regards,
> Stefano Babic

Cheers
Wolfgang Denk - July 13, 2011, 6:29 p.m.
Dear Marek Vasut,

In message <201107131912.45580.marek.vasut@gmail.com> you wrote:
>
> > You have added an array with fixed values as indicated in the
> > Freescale's manual (Table 26-7 for i.MX31, Table 40-7 for MX51, Table
> > 41-12 for MX53). What about to add also some comments about these changes ?
> 
> I stole this from linux kernel, but sure ... as this is nearly a complete 
> rewrite, I didn't describe all the changes.

In this case you are supposed toprovide proper attribution.  Please
see http://www.denx.de/wiki/view/U-Boot/Patches#Attributing_Code_Copyrights_Sign

Please resubmit with this additional information.


Best regards,

Wolfgang Denk
Marek Vasut - July 13, 2011, 6:32 p.m.
> Dear Marek Vasut,
> 
> In message <201107131912.45580.marek.vasut@gmail.com> you wrote:
> > 
> > > You have added an array with fixed values as indicated in the
> > > Freescale's manual (Table 26-7 for i.MX31, Table 40-7 for MX51, Table
> > > 41-12 for MX53). What about to add also some comments about these
> > > changes ?
> > 
> > I stole this from linux kernel, but sure ... as this is nearly a
> > complete   rewrite, I didn't describe all the changes.
> 
> In this case you are supposed toprovide proper attribution.   Please
> see
> http://www.denx.de/wiki/view/U-Boot/Patches#Attributing_Code_Copyrights_Sign
> 
> Please resubmit with this additional information.
> 

Sorry, will be in V3. Some people just learn the hard way :-/

> 
> Best regards,
> 
> Wolfgang Denk
> 
> -- 
> DENX Software Engineering GmbH,         MD: Wolfgang Denk & Detlev Zundel
> HRB 165235 Munich, Office: Kirchenstr.5, D-82194 Groebenzell, Germany
> Phone: (+49)-8142-66989-10 Fax: (+49)-8142-66989-80 Email: wd@denx.de
> Defaults are wonderful, just like fire.
>                                     - Larry Wall in <1996Mar6.004121.27890@netlabs.com>
Marek Vasut - July 13, 2011, 9:51 p.m.
On Wednesday, July 13, 2011 03:56:45 PM Stefano Babic wrote:
> On 07/13/2011 11:53 AM, Marek Vasut wrote:
> > Rewrite the mxc_i2c driver.
> > 
-- snip --
> > -	while (!(readw(I2C_BASE + I2SR) & I2SR_IBB) && --timeout)
> > +	while (timeout--) {
> > +		if (readb(I2C_BASE + I2SR) & I2SR_IIF) {
> 
> If we wait for completion, should we not check the ICF bit instead of
> IIF, as done before your patch ?

Hi,

Stefano, for some reason, when I check for ICF, the i2c driver doesn't work. 
With IIF it does (and when IIF is asserted, so is ICF). Maybe ICF is asserted 
earlier or who knows.
> 
> > +/*
> > + * Start the controller
> > + */

-- snip --
Heiko Schocher - July 14, 2011, 8:58 a.m.
Hello Stefano,

Stefano Babic wrote:
> On 07/13/2011 11:53 AM, 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>
>> ---
> 
> Hi Marek,
> 
> I have added Heiko in CC. He is the Maintainer for I2C.

Thanks! ;-)

[...]
>> +/*
>> + * 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())
>> +	int ret;
>> +	int i;
>> +
> 
> mmmhh...it seems to me you change completely the logic here. Heiko, waht
> do you think about ?

I think this is OK, because of calling i2c_imx_trx_complete() later,
which do this ...

bye,
Heiko

Patch

diff --git a/drivers/i2c/mxc_i2c.c b/drivers/i2c/mxc_i2c.c
index 89d1973..449d765 100644
--- a/drivers/i2c/mxc_i2c.c
+++ b/drivers/i2c/mxc_i2c.c
@@ -68,218 +68,351 @@ 
 #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
+ *
+ * FIXME: remove #ifdefs !
+ */
+static void i2c_imx_set_clk(unsigned int rate)
 {
-	int freq;
+	unsigned int i2c_clk_rate;
+	unsigned int div;
 	int i;
 
+	/* Divider value calculation */
 #if defined(CONFIG_MX31)
 	struct clock_control_regs *sc_regs =
 		(struct clock_control_regs *)CCM_BASE;
 
-	freq = mx31_get_ipg_clk();
+	i2c_clk_rate = mx31_get_ipg_clk();
 	/* start the required I2C clock */
 	writel(readl(&sc_regs->cgr0) | (3 << I2C_CLK_OFFSET),
 		&sc_regs->cgr0);
 #else
-	freq = mxc_get_clock(MXC_IPG_PERCLK);
+	i2c_clk_rate = mxc_get_clock(MXC_IPG_CLK);
 #endif
+	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 */
+	writeb(div, I2C_BASE + IFDR);
+	clk_idx = div;
+}
 
-	for (i = 0; i < 0x1f; i++)
-		if (freq / div[i] <= speed)
-			break;
-
-	debug("%s: speed: %d\n", __func__, speed);
+/*
+ * Reset I2C Controller
+ */
+void i2c_reset(void)
+{
+	writeb(0, I2C_BASE + I2CR);	/* Reset module */
+	writeb(0, I2C_BASE + 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)
 {
+	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_BASE + 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)
 {
 	int timeout = I2C_MAX_TIMEOUT;
 
-	while (!(readw(I2C_BASE + I2SR) & I2SR_IBB) && --timeout)
+	while (timeout--) {
+		if (readb(I2C_BASE + I2SR) & I2SR_IIF) {
+			writeb(0, I2C_BASE + 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;
+	return readb(I2C_BASE + I2SR) & I2SR_RX_NO_AK;
+}
 
-	while ((!(readw(I2C_BASE + I2SR) & I2SR_ICF)) && (--timeout)) {
-		writew(0, I2C_BASE + I2SR);
-		udelay(1);
-	}
-	udelay(200);
+/*
+ * Start the controller
+ */
+int i2c_imx_start(void)
+{
+	unsigned int temp = 0;
+	int result;
 
-	writew(0, I2C_BASE + I2SR);	/* clear interrupt */
+	writeb(clk_idx, I2C_BASE + IFDR);
 
-	return timeout;
-}
+	/* Enable I2C controller */
+	writeb(0, I2C_BASE + I2SR);
+	writeb(I2CR_IEN, I2C_BASE + I2CR);
 
+	/* Wait controller to be stable */
+	udelay(50);
 
-static int tx_byte(u8 byte)
-{
-	writew(byte, I2C_BASE + I2DR);
+	/* Start I2C transaction */
+	temp = readb(I2C_BASE + I2CR);
+	temp |= I2CR_MSTA;
+	writeb(temp, I2C_BASE + I2CR);
+
+	result = i2c_imx_bus_busy(1);
+	if (result)
+		return result;
+
+	temp |= I2CR_MTX | I2CR_TX_NO_AK;
+	writeb(temp, I2C_BASE + 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()
 {
-	if (!wait_complete())
-		return -1;
+	unsigned int temp = 0;
 
-	if (last)
-		writew(I2CR_IEN, I2C_BASE + I2CR);
+	/* Stop I2C transaction */
+	temp = readb(I2C_BASE + I2CR);
+	temp |= ~(I2CR_MSTA | I2CR_MTX);
+	writeb(temp, I2C_BASE + I2CR);
 
-	return readw(I2C_BASE + I2DR);
+	i2c_imx_bus_busy(0);
+
+	/* Disable I2C controller */
+	writeb(0, I2C_BASE + 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)
 {
 	int ret;
 
-	writew(0, I2C_BASE + I2CR); /* Reset module */
-	writew(I2CR_IEN, I2C_BASE + I2CR);
+	writeb((chip << 1) | read, I2C_BASE + 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())
+	int ret;
+	int i;
+
+	for (i = 0; i < (8 * alen); i += 8) {
+		writeb((addr >> i) & 0xff, I2C_BASE + 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)
+{
+	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_BASE + I2CR);
+	temp |= I2CR_RSTA;
+	writeb(temp, I2C_BASE + I2CR);
+
+	ret = i2c_imx_set_chip_addr(chip, 1);
+	if (ret)
+		return ret;
+
+	/* setup bus to read data */
+	temp = readb(I2C_BASE + I2CR);
+	temp &= ~I2CR_MTX;
+	if (len == 1)
+		temp &= ~I2CR_TX_NO_AK;
+	writeb(temp, I2C_BASE + I2CR);
+	readb(I2C_BASE + 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_BASE + I2CR);
+			temp &= ~(I2CR_MSTA | I2CR_MTX);
+			writeb(temp, I2C_BASE + I2CR);
+			i2c_imx_bus_busy(0);
+		} else if (i == (len - 2)) {
+			temp = readb(I2C_BASE + I2CR);
+			temp |= I2CR_TX_NO_AK;
+			writeb(temp, I2C_BASE + 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_BASE + 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);
+	int ret;
+	unsigned int temp;
+	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_BASE + 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 */