i2c: imx: add slave support. v2 status

Message ID 2404b260-4bd6-8ff6-3898-c9c63aaf6855@dev.rtsoft.ru
State New
Headers show

Commit Message

Maxim Syrchin Oct. 27, 2016, 7:38 p.m.
Hi,
Sorry for huge delay in answering. Unfortunately we don't have enough 
resources now to prepare clean enough patch to be accepted by community.
Please find the latest version attached.  Driver has passed stress 
tests, but looks like it need seriuos refactoring (it is unnecessarily 
complicated).
We still have polling in slave code. Since imx doesn't generate 
interrupt on "bus busy"/"bus free" events we have to test IBB bit in 
polling loop.
Comparing to v2 version several race conditions were fixed (bus locking 
by slave, breaking slave transaction by starting master xfer). v2 is 
working pretty good in slave-only or master-only mode. It is reasonable 
to add  slave locking test  - sometimes imx6 hw doesn't release data 
line. As workaround we use dummy reading of IMX_I2C_I2DR if driver is 
in  slave mode and IBB bit is set for a long time.

Thanks,
Maxim Syrchin


27.10.2016 10:31, Frkuska, Joshua пишет:
> Hi Maxim, Dmitriy, Wolfram,
>
> If there is no immediate plan for a third release of the below patch 
> set, would it be possible to continue with merging v2 after addressing 
> the remaining concerns?
>
>
> Thank you and regards,
>
> Joshua
>> Hi Maxim,
>>
>> On 2016-03-04 11:06:10 in the thread "Re: [PATCH] i2c: imx: add slave 
>> support. v2"
>> referenced here:   https://patchwork.ozlabs.org/patch/573353/ you said:
>>> Hi Wolfram,
>>> I'm now working on creating new driver version. I think I'll be able to
>>> sent it soon.
>> Do you still plan to release a driver update for an i2c imx driver 
>> slave support?
>>
>> Best regards,
>> Jim Baxter
>>
From 61ae34268d78eb284bf8ee0cbe8f9f0c5e7df074 Mon Sep 17 00:00:00 2001
From: Maxim Syrchin <msyrchin@dev.rtsoft.ru>
Date: Thu, 27 Oct 2016 17:37:56 +0300
Subject: [PATCH] i2c: imx: add slave support. v3

Add I2C slave provider using the generic slave interface.
It also supports master transactions when the slave in the idle mode.

Signed-off-by: Maxim Syrchin <msyrchin@dev.rtsoft.ru>
---
 drivers/i2c/busses/i2c-imx.c | 682 +++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 653 insertions(+), 29 deletions(-)

Comments

Frkuska, Joshua Oct. 31, 2016, 2:14 a.m. | #1
Hello Maxim,

Thank you very much for the intermediate patch. I am in the process of 
reviewing it. Please let me clarify a few questions I have.

1. What alternative to "bus busy/bus free/IBB" polling do you have in 
mind? This seems like a reasonable approach to me.
2. What are the major points you consider in need of refactoring?
3. You mention race conditions being fixed in this version relating to 
bus-locking by the slave and breaking slave transactions by the master. 
Does this mean mixed slave/master mode works to your satisfaction? If 
not, what work still needs to be done here?
4. You mention the need for a slave locking test and a work-around 
(checking IMX_I2C_I2DR and IBB) being in-place. Why is this work-around 
not sufficient?

Thanks again,

Joshua


On 10/28/2016 04:38 AM, Maxim Syrchin wrote:
> Hi,
> Sorry for huge delay in answering. Unfortunately we don't have enough 
> resources now to prepare clean enough patch to be accepted by community.
> Please find the latest version attached.  Driver has passed stress 
> tests, but looks like it need seriuos refactoring (it is unnecessarily 
> complicated).
> We still have polling in slave code. Since imx doesn't generate 
> interrupt on "bus busy"/"bus free" events we have to test IBB bit in 
> polling loop.
> Comparing to v2 version several race conditions were fixed (bus 
> locking by slave, breaking slave transaction by starting master xfer). 
> v2 is working pretty good in slave-only or master-only mode. It is 
> reasonable to add  slave locking test  - sometimes imx6 hw doesn't 
> release data line. As workaround we use dummy reading of IMX_I2C_I2DR 
> if driver is in  slave mode and IBB bit is set for a long time.
>
> Thanks,
> Maxim Syrchin
>
>
> 27.10.2016 10:31, Frkuska, Joshua пишет:
>> Hi Maxim, Dmitriy, Wolfram,
>>
>> If there is no immediate plan for a third release of the below patch 
>> set, would it be possible to continue with merging v2 after 
>> addressing the remaining concerns?
>>
>>
>> Thank you and regards,
>>
>> Joshua
>>> Hi Maxim,
>>>
>>> On 2016-03-04 11:06:10 in the thread "Re: [PATCH] i2c: imx: add 
>>> slave support. v2"
>>> referenced here:   https://patchwork.ozlabs.org/patch/573353/ you said:
>>>> Hi Wolfram,
>>>> I'm now working on creating new driver version. I think I'll be 
>>>> able to
>>>> sent it soon.
>>> Do you still plan to release a driver update for an i2c imx driver 
>>> slave support?
>>>
>>> Best regards,
>>> Jim Baxter
>>>
>
Maxim Syrchin Oct. 31, 2016, 6:21 p.m. | #2
Hello,

Please find some comments below.


31.10.2016 5:14, Frkuska, Joshua пишет:
> Hello Maxim,
>
> Thank you very much for the intermediate patch. I am in the process of 
> reviewing it. Please let me clarify a few questions I have.
>
> 1. What alternative to "bus busy/bus free/IBB" polling do you have in 
> mind? This seems like a reasonable approach to me.
We didn't find any suitable alternatives. The only one we're considered 
was using timeout on receive (which is kind of polling of course)
> 2. What are the major points you consider in need of refactoring?
As you can see we have implemented FSM in slave thread.
- Due to lack of time all master functionality had not been included in 
State Machine.
- wait_event_timeout() calls are used in every event handler (obviosly 
it is better to have only one wait function)
- Need to review state switching code
> 3. You mention race conditions being fixed in this version relating to 
> bus-locking by the slave and breaking slave transactions by the 
> master. Does this mean mixed slave/master mode works to your 
> satisfaction? If not, what work still needs to be done here?
Yes mixed slave/master mode works ok. It had passed long-lasting stress 
tests (async message exchange of two imx6 boards connected together by 
i2c bus )
> 4. You mention the need for a slave locking test and a work-around 
> (checking IMX_I2C_I2DR and IBB) being in-place. Why is this 
> work-around not sufficient?
By the time we discovered I2DR workaround we went far from version 2 of 
driver and it wasn't tested. I'm sure that I2DR workaround will improve 
stability, but I do not know if it will fix all issues (i.e. passing of 
stress tests )


Best Regards,
Maxim Syrchin
> Thanks again,
>
> Joshua
>
>
> On 10/28/2016 04:38 AM, Maxim Syrchin wrote:
>> Hi,
>> Sorry for huge delay in answering. Unfortunately we don't have enough 
>> resources now to prepare clean enough patch to be accepted by community.
>> Please find the latest version attached.  Driver has passed stress 
>> tests, but looks like it need seriuos refactoring (it is 
>> unnecessarily complicated).
>> We still have polling in slave code. Since imx doesn't generate 
>> interrupt on "bus busy"/"bus free" events we have to test IBB bit in 
>> polling loop.
>> Comparing to v2 version several race conditions were fixed (bus 
>> locking by slave, breaking slave transaction by starting master 
>> xfer). v2 is working pretty good in slave-only or master-only mode. 
>> It is reasonable to add  slave locking test  - sometimes imx6 hw 
>> doesn't release data line. As workaround we use dummy reading of 
>> IMX_I2C_I2DR if driver is in  slave mode and IBB bit is set for a 
>> long time.
>>
>> Thanks,
>> Maxim Syrchin
>>
>>
>> 27.10.2016 10:31, Frkuska, Joshua пишет:
>>> Hi Maxim, Dmitriy, Wolfram,
>>>
>>> If there is no immediate plan for a third release of the below patch 
>>> set, would it be possible to continue with merging v2 after 
>>> addressing the remaining concerns?
>>>
>>>
>>> Thank you and regards,
>>>
>>> Joshua
>>>> Hi Maxim,
>>>>
>>>> On 2016-03-04 11:06:10 in the thread "Re: [PATCH] i2c: imx: add 
>>>> slave support. v2"
>>>> referenced here: https://patchwork.ozlabs.org/patch/573353/ you said:
>>>>> Hi Wolfram,
>>>>> I'm now working on creating new driver version. I think I'll be 
>>>>> able to
>>>>> sent it soon.
>>>> Do you still plan to release a driver update for an i2c imx driver 
>>>> slave support?
>>>>
>>>> Best regards,
>>>> Jim Baxter
>>>>
>>
>

--
To unsubscribe from this list: send the line "unsubscribe linux-i2c" in
the body of a message to majordomo@vger.kernel.org
More majordomo info at  http://vger.kernel.org/majordomo-info.html
Frkuska, Joshua Dec. 1, 2016, 8:11 a.m. | #3
Hello Maxim,

I have a few questions for you. Please see my comments inline below.

In addition, I have modified your patch-set slightly and I would like to 
progress it to merger if you do not have any issues with this. I would 
like to sync with you here before moving forward and submitting a new patch.

Thank you and best regards,

Joshua

On 11/01/2016 03:21 AM, Maxim Syrchin wrote:
> Hello,
>
> Please find some comments below.
>
>
> 31.10.2016 5:14, Frkuska, Joshua пишет:
>> Hello Maxim,
>>
>> Thank you very much for the intermediate patch. I am in the process 
>> of reviewing it. Please let me clarify a few questions I have.
>>
>> 1. What alternative to "bus busy/bus free/IBB" polling do you have in 
>> mind? This seems like a reasonable approach to me.
> We didn't find any suitable alternatives. The only one we're 
> considered was using timeout on receive (which is kind of polling of 
> course)
>> 2. What are the major points you consider in need of refactoring?
> As you can see we have implemented FSM in slave thread.
> - Due to lack of time all master functionality had not been included 
> in State Machine.
Currently there seems to not be a problem of entirely handling master 
functionality in the slave state machine as it is handled outside of it. 
Do you feel everything should be handled in the slave state machine? I 
dont see any holes in the logic currently.
> - wait_event_timeout() calls are used in every event handler (obviosly 
> it is better to have only one wait function)
It is possible to have a single wait_event_timeout call at the expense 
of a bit of conditional logic in i2c_imx_slave_threadfn but this brings 
me to my next comment
> - Need to review state switching code
I have reviewed all states in a state transition table and all of them 
seem well defined. My only question here is in regards to the STATE_SP 
state. I would like to understand your motivation for it. To me it seems 
like this can also be handled in STATE_IDLE but I would like to get your 
reasoning behind it
>> 3. You mention race conditions being fixed in this version relating 
>> to bus-locking by the slave and breaking slave transactions by the 
>> master. Does this mean mixed slave/master mode works to your 
>> satisfaction? If not, what work still needs to be done here?
> Yes mixed slave/master mode works ok. It had passed long-lasting 
> stress tests (async message exchange of two imx6 boards connected 
> together by i2c bus )
>> 4. You mention the need for a slave locking test and a work-around 
>> (checking IMX_I2C_I2DR and IBB) being in-place. Why is this 
>> work-around not sufficient?
> By the time we discovered I2DR workaround we went far from version 2 
> of driver and it wasn't tested. I'm sure that I2DR workaround will 
> improve stability, but I do not know if it will fix all issues (i.e. 
> passing of stress tests )
>
>
> Best Regards,
> Maxim Syrchin
>> Thanks again,
>>
>> Joshua
>>
>>
>> On 10/28/2016 04:38 AM, Maxim Syrchin wrote:
>>> Hi,
>>> Sorry for huge delay in answering. Unfortunately we don't have 
>>> enough resources now to prepare clean enough patch to be accepted by 
>>> community.
>>> Please find the latest version attached.  Driver has passed stress 
>>> tests, but looks like it need seriuos refactoring (it is 
>>> unnecessarily complicated).
>>> We still have polling in slave code. Since imx doesn't generate 
>>> interrupt on "bus busy"/"bus free" events we have to test IBB bit in 
>>> polling loop.
>>> Comparing to v2 version several race conditions were fixed (bus 
>>> locking by slave, breaking slave transaction by starting master 
>>> xfer). v2 is working pretty good in slave-only or master-only mode. 
>>> It is reasonable to add  slave locking test - sometimes imx6 hw 
>>> doesn't release data line. As workaround we use dummy reading of 
>>> IMX_I2C_I2DR if driver is in  slave mode and IBB bit is set for a 
>>> long time.
>>>
>>> Thanks,
>>> Maxim Syrchin
>>>
>>>
>>> 27.10.2016 10:31, Frkuska, Joshua пишет:
>>>> Hi Maxim, Dmitriy, Wolfram,
>>>>
>>>> If there is no immediate plan for a third release of the below 
>>>> patch set, would it be possible to continue with merging v2 after 
>>>> addressing the remaining concerns?
>>>>
>>>>
>>>> Thank you and regards,
>>>>
>>>> Joshua
>>>>> Hi Maxim,
>>>>>
>>>>> On 2016-03-04 11:06:10 in the thread "Re: [PATCH] i2c: imx: add 
>>>>> slave support. v2"
>>>>> referenced here: https://patchwork.ozlabs.org/patch/573353/ you said:
>>>>>> Hi Wolfram,
>>>>>> I'm now working on creating new driver version. I think I'll be 
>>>>>> able to
>>>>>> sent it soon.
>>>>> Do you still plan to release a driver update for an i2c imx driver 
>>>>> slave support?
>>>>>
>>>>> Best regards,
>>>>> Jim Baxter
>>>>>
>>>
>>
>
--
To unsubscribe from this list: send the line "unsubscribe linux-i2c" in
the body of a message to majordomo@vger.kernel.org
More majordomo info at  http://vger.kernel.org/majordomo-info.html

Patch

diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index 592a8f2..11a2292 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -53,6 +53,7 @@ 
 #include <linux/pm_runtime.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
+#include <linux/kthread.h>
 
 /* This will be the driver name the kernel reports */
 #define DRIVER_NAME "imx-i2c"
@@ -171,6 +172,82 @@  enum imx_i2c_type {
 	VF610_I2C,
 };
 
+enum i2c_imx_state {
+	STATE_IDLE = 0,
+	STATE_SLAVE,
+	STATE_MASTER,
+	STATE_SP
+};
+
+#define MAX_EVENTS (1<<4)
+#define EUNDEFINED 4000
+
+enum i2c_imx_events {
+	EVT_AL = 0,
+	EVT_SI,
+	EVT_START,
+	EVT_STOP,
+	EVT_POLL,
+	EVT_INVALID,
+	EVT_ENTRY
+};
+
+typedef struct evt_queue {
+	atomic_t count;
+	atomic_t ir;
+	atomic_t iw;
+	unsigned int evt[MAX_EVENTS];
+} evt_queue;
+
+static inline int evt_find_next_idx(atomic_t *v)
+{
+	return atomic_inc_return(v) & (MAX_EVENTS - 1);
+}
+
+static unsigned int evt_put(evt_queue *stk, unsigned int evt)
+{
+	int count = atomic_inc_return(&stk->count);
+	int idx;
+	if (count < MAX_EVENTS)
+	{
+		idx = evt_find_next_idx(&stk->iw);
+		stk->evt[idx] = evt;
+
+		return 0;
+	} else {
+		atomic_dec(&stk->count);
+		return EVT_INVALID;
+	}
+}
+
+static unsigned int evt_get(evt_queue *stk)
+{
+	int count = atomic_dec_return(&stk->count);
+	int idx;
+
+	if (count >= 0)
+	{
+		idx = evt_find_next_idx(&stk->ir);
+		return stk->evt[idx];
+	} else {
+		atomic_inc(&stk->count);
+		return EVT_INVALID;
+	}
+}
+
+static unsigned int evt_is_empty(evt_queue *stk)
+{
+	int ret = atomic_read(&stk->count);
+	return (ret <= 0);
+}
+
+static void evt_init(evt_queue *stk)
+{
+	atomic_set(&stk->count,0);
+	atomic_set(&stk->iw,0);
+	atomic_set(&stk->ir,0);
+}
+
 struct imx_i2c_hwdata {
 	enum imx_i2c_type	devtype;
 	unsigned		regshift;
@@ -193,6 +270,7 @@  struct imx_i2c_dma {
 
 struct imx_i2c_struct {
 	struct i2c_adapter	adapter;
+	struct i2c_client	*slave;
 	struct clk		*clk;
 	void __iomem		*base;
 	wait_queue_head_t	queue;
@@ -210,6 +288,18 @@  struct imx_i2c_struct {
 	struct pinctrl_state *pinctrl_pins_gpio;
 
 	struct imx_i2c_dma	*dma;
+
+	unsigned int		state;
+	evt_queue		events;
+	atomic_t		last_error;
+
+	int			master_interrupt;
+	int			start_retry_cnt;
+	int			slave_poll_cnt;
+
+	struct task_struct	*slave_task;
+	wait_queue_head_t	state_queue;
+
 };
 
 static const struct imx_i2c_hwdata imx1_i2c_hwdata = {
@@ -414,17 +504,31 @@  static void i2c_imx_dma_free(struct imx_i2c_struct *i2c_imx)
 static int i2c_imx_bus_busy(struct imx_i2c_struct *i2c_imx, int for_busy)
 {
 	unsigned long orig_jiffies = jiffies;
-	unsigned int temp;
+	unsigned int temp, ctl;
+
 
 	dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
 
 	while (1) {
 		temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2SR);
+		ctl = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
 
-		/* check for arbitration lost */
-		if (temp & I2SR_IAL) {
-			temp &= ~I2SR_IAL;
-			imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2SR);
+		/*
+			Check for arbitration lost. Datasheet recommends to
+			clean IAL bit in interrupt handler before any other action.
+			So, we cannot handle IAL here like this:
+			if (temp & I2SR_IAL) {
+				tepm &= ~I2SR_IAL;
+				imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2SR);
+			}
+
+			But we can detect if controller resets MSTA bit, because
+			hardware is switched to slave mode if arbitration was lost.
+		*/
+
+		if (for_busy && !(ctl & I2CR_MSTA)) {
+			dev_dbg(&i2c_imx->adapter.dev,
+				"<%s> Lost arbitration (SR = %02x, CR = %02x)\n", __func__, temp, ctl);
 			return -EAGAIN;
 		}
 
@@ -445,14 +549,14 @@  static int i2c_imx_bus_busy(struct imx_i2c_struct *i2c_imx, int for_busy)
 
 static int i2c_imx_trx_complete(struct imx_i2c_struct *i2c_imx)
 {
-	wait_event_timeout(i2c_imx->queue, i2c_imx->i2csr & I2SR_IIF, HZ / 10);
+	wait_event_timeout(i2c_imx->queue, i2c_imx->master_interrupt, HZ / 10);
 
-	if (unlikely(!(i2c_imx->i2csr & I2SR_IIF))) {
+	if (unlikely(!(i2c_imx->master_interrupt))) {
 		dev_dbg(&i2c_imx->adapter.dev, "<%s> Timeout\n", __func__);
 		return -ETIMEDOUT;
 	}
 	dev_dbg(&i2c_imx->adapter.dev, "<%s> TRX complete\n", __func__);
-	i2c_imx->i2csr = 0;
+	i2c_imx->master_interrupt = 0;
 	return 0;
 }
 
@@ -510,6 +614,44 @@  static void i2c_imx_set_clk(struct imx_i2c_struct *i2c_imx)
 #endif
 }
 
+static int i2c_imx_configure_clock(struct imx_i2c_struct *i2c_imx)
+{
+	int result;
+
+	i2c_imx_set_clk(i2c_imx);
+
+	result = clk_prepare_enable(i2c_imx->clk);
+	if (result == 0)
+		imx_i2c_write_reg(i2c_imx->ifdr, i2c_imx, IMX_I2C_IFDR);
+
+	return result;
+}
+
+static void i2c_imx_enable_i2c_controller(struct imx_i2c_struct *i2c_imx)
+{
+	imx_i2c_write_reg(i2c_imx->hwdata->i2sr_clr_opcode, i2c_imx,
+		IMX_I2C_I2SR);
+	imx_i2c_write_reg(i2c_imx->hwdata->i2cr_ien_opcode, i2c_imx,
+		IMX_I2C_I2CR);
+
+	/* Wait controller to be stable */
+	udelay(50);
+}
+
+static int i2c_imx_hw_start(struct imx_i2c_struct *i2c_imx)
+{
+	int result;
+
+	dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
+
+	result = i2c_imx_configure_clock(i2c_imx);
+	if (result != 0)
+		return result;
+	i2c_imx_enable_i2c_controller(i2c_imx);
+	return 0;
+}
+
+
 static int i2c_imx_start(struct imx_i2c_struct *i2c_imx)
 {
 	unsigned int temp = 0;
@@ -517,20 +659,49 @@  static int i2c_imx_start(struct imx_i2c_struct *i2c_imx)
 
 	dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
 
-	i2c_imx_set_clk(i2c_imx);
+	if (i2c_imx->slave_task != NULL)
+	{
+		int cnt = 0;
+		atomic_set(&i2c_imx->last_error, EUNDEFINED);
+		if (evt_put(&i2c_imx->events, EVT_START) != 0)
+		{
+			dev_err(&i2c_imx->adapter.dev, "Event buffer overflow\n");
+			return -EBUSY;
+		}
 
-	imx_i2c_write_reg(i2c_imx->ifdr, i2c_imx, IMX_I2C_IFDR);
-	/* Enable I2C controller */
-	imx_i2c_write_reg(i2c_imx->hwdata->i2sr_clr_opcode, i2c_imx, IMX_I2C_I2SR);
-	imx_i2c_write_reg(i2c_imx->hwdata->i2cr_ien_opcode, i2c_imx, IMX_I2C_I2CR);
+		wake_up(&i2c_imx->state_queue);
 
-	/* Wait controller to be stable */
-	usleep_range(50, 150);
+		while( (result = atomic_read(&i2c_imx->last_error)) == EUNDEFINED)
+		{
+			schedule();
+
+		/* TODO: debug workaround - start hung monitoring */
+			cnt++;
+			if (cnt == 500000)
+			{
+				dev_err(&i2c_imx->adapter.dev, "Too many start loops\n");
+				imx_i2c_write_reg(i2c_imx->hwdata->i2cr_ien_opcode ^ I2CR_IEN,
+						i2c_imx, IMX_I2C_I2CR);
+				i2c_imx_enable_i2c_controller(i2c_imx);
+				imx_i2c_write_reg(i2c_imx->hwdata->i2cr_ien_opcode | I2CR_IIEN,
+						i2c_imx,IMX_I2C_I2CR);
+
+				return -ETIMEDOUT;
+			}
+
+		};
+		return result;
+	}
+
+	result = i2c_imx_hw_start(i2c_imx);
+	if (result != 0)
+		return result;
 
 	/* Start I2C transaction */
 	temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
 	temp |= I2CR_MSTA;
 	imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
+
 	result = i2c_imx_bus_busy(i2c_imx, 1);
 	if (result)
 		return result;
@@ -542,10 +713,23 @@  static int i2c_imx_start(struct imx_i2c_struct *i2c_imx)
 	return result;
 }
 
-static void i2c_imx_stop(struct imx_i2c_struct *i2c_imx)
+static void i2c_imx_slave_init(struct imx_i2c_struct *i2c_imx)
 {
-	unsigned int temp = 0;
+	unsigned int temp;
+
+	dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
+
+	i2c_imx_enable_i2c_controller(i2c_imx);
+
+	/* Set Slave mode with interrupt enable */
+	temp = i2c_imx->hwdata->i2cr_ien_opcode | I2CR_IIEN;
+	imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
+}
 
+
+static void i2c_imx_hw_stop(struct imx_i2c_struct *i2c_imx)
+{
+	unsigned int temp = 0;
 	if (!i2c_imx->stopped) {
 		/* Stop I2C transaction */
 		dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
@@ -555,6 +739,7 @@  static void i2c_imx_stop(struct imx_i2c_struct *i2c_imx)
 			temp &= ~I2CR_DMAEN;
 		imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
 	}
+
 	if (is_imx1_i2c(i2c_imx)) {
 		/*
 		 * This delay caused by an i.MXL hardware bug.
@@ -568,24 +753,388 @@  static void i2c_imx_stop(struct imx_i2c_struct *i2c_imx)
 		i2c_imx->stopped = 1;
 	}
 
-	/* Disable I2C controller */
-	temp = i2c_imx->hwdata->i2cr_ien_opcode ^ I2CR_IEN,
+	temp = i2c_imx->hwdata->i2cr_ien_opcode ^ I2CR_IEN;
 	imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
+
+	clk_disable_unprepare(i2c_imx->clk);
+
+}
+
+static void i2c_imx_stop(struct imx_i2c_struct *i2c_imx)
+{
+	if (i2c_imx->slave == NULL) {
+		i2c_imx_hw_stop(i2c_imx);
+	} else {
+
+		dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
+
+		evt_put(&i2c_imx->events, EVT_STOP);
+		wake_up(&i2c_imx->state_queue);
+	}
+}
+
+static void i2c_imx_clear_isr_bit(struct imx_i2c_struct *i2c_imx,
+	unsigned int status)
+{
+	status &= ~I2SR_IIF;
+	status |= (i2c_imx->hwdata->i2sr_clr_opcode & I2SR_IIF);
+	imx_i2c_write_reg(status, i2c_imx, IMX_I2C_I2SR);
+}
+
+static void i2c_imx_clear_ial_bit(struct imx_i2c_struct *i2c_imx)
+{
+	unsigned int status;
+	status = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2SR);
+	status &= ~I2SR_IAL;
+	imx_i2c_write_reg(status, i2c_imx, IMX_I2C_I2SR);
+}
+
+static void set_state (struct imx_i2c_struct *i2c_imx, unsigned int new);
+
+static void i2c_imx_slave_process_irq(struct imx_i2c_struct *i2c_imx)
+{
+	unsigned int status, ctl;
+	u8 data;
+
+	status = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2SR);
+	ctl = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
+
+	if (status & I2SR_IAAS) {
+		if (status & I2SR_SRW) {
+			/* master wants to read from us */
+			i2c_slave_event(i2c_imx->slave,
+				I2C_SLAVE_READ_REQUESTED, &data);
+			ctl |= I2CR_MTX;
+			imx_i2c_write_reg(ctl, i2c_imx, IMX_I2C_I2CR);
+
+			/*send data */
+			imx_i2c_write_reg(data, i2c_imx, IMX_I2C_I2DR);
+		} else {
+			dev_dbg(&i2c_imx->adapter.dev, "write requested");
+			i2c_slave_event(i2c_imx->slave,
+				I2C_SLAVE_WRITE_REQUESTED, &data);
+			/*slave receive */
+			ctl &= ~I2CR_MTX;
+			imx_i2c_write_reg(ctl, i2c_imx, IMX_I2C_I2CR);
+
+			/*dummy read */
+			data = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2DR);
+		}
+	} else {
+		/* slave send */
+		if (ctl & I2CR_MTX) {
+			if (!(status & I2SR_RXAK)) {	/*ACK received */
+				i2c_slave_event(i2c_imx->slave,
+					I2C_SLAVE_READ_PROCESSED, &data);
+				ctl |= I2CR_MTX;
+				imx_i2c_write_reg(ctl, i2c_imx, IMX_I2C_I2CR);
+				/*send data */
+				imx_i2c_write_reg(data, i2c_imx, IMX_I2C_I2DR);
+			} else {
+				/*no ACK. */
+				/*dummy read */
+				dev_dbg(&i2c_imx->adapter.dev, "read requested");
+				i2c_slave_event(i2c_imx->slave,
+					I2C_SLAVE_READ_REQUESTED, &data);
+
+				ctl &= ~I2CR_MTX;
+				imx_i2c_write_reg(ctl, i2c_imx, IMX_I2C_I2CR);
+				imx_i2c_read_reg(i2c_imx, IMX_I2C_I2DR);
+			}
+		} else {	/*read */
+			ctl &= ~I2CR_MTX;
+			imx_i2c_write_reg(ctl, i2c_imx, IMX_I2C_I2CR);
+
+			/*read */
+			data = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2DR);
+			dev_dbg(&i2c_imx->adapter.dev, "received %x",
+				(unsigned int) data);
+			i2c_slave_event(i2c_imx->slave,
+				I2C_SLAVE_WRITE_RECEIVED, &data);
+		}
+	}
+}
+
+
+static void idle_evt_handler (struct imx_i2c_struct *i2c_imx, unsigned int event)
+{
+	u8 reg, data;
+	switch (event) {
+		case EVT_ENTRY:
+			imx_i2c_write_reg(i2c_imx->hwdata->i2cr_ien_opcode ^ I2CR_IEN,
+					i2c_imx, IMX_I2C_I2CR);
+			i2c_imx_enable_i2c_controller(i2c_imx);
+			imx_i2c_write_reg(i2c_imx->hwdata->i2cr_ien_opcode | I2CR_IIEN,
+					i2c_imx,IMX_I2C_I2CR);
+			if (atomic_read(&i2c_imx->last_error) == EUNDEFINED)
+			{
+				dev_dbg(&i2c_imx->adapter.dev, "Reset lost START event\n");
+				atomic_set(&i2c_imx->last_error, -EBUSY);
+			}
+			i2c_imx->start_retry_cnt = 0 ;
+			return;
+		case EVT_AL:
+			i2c_imx_clear_ial_bit(i2c_imx);
+			break;
+		case EVT_SI:
+			set_state(i2c_imx, STATE_SLAVE);
+			i2c_imx_slave_process_irq(i2c_imx);
+			break;
+		case EVT_START:
+			reg = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2SR);
+			if ((reg & I2SR_IBB) != 0) {
+				atomic_set(&i2c_imx->last_error, -EBUSY);
+				break;
+			}
+
+			reg = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
+			reg |= I2CR_MSTA;
+			imx_i2c_write_reg(reg, i2c_imx, IMX_I2C_I2CR);
+			set_state(i2c_imx,STATE_SP);
+			i2c_imx->start_retry_cnt = 0;
+			return;
+			break;
+		case EVT_STOP:
+			break;
+		case EVT_POLL:
+			break;
+		default:
+			break;
+	}
+
+	wait_event_interruptible_timeout(i2c_imx->state_queue,(evt_is_empty(&i2c_imx->events) == 0),HZ/10);
+
+}
+
+static void master_evt_handler (struct imx_i2c_struct *i2c_imx, unsigned int event)
+{
+	switch (event) {
+		case EVT_ENTRY:
+			i2c_imx->start_retry_cnt = 0 ;
+			return;
+		case EVT_AL:
+			set_state(i2c_imx, STATE_IDLE);
+			break;
+		case EVT_SI:
+			break;
+		case EVT_START:
+			atomic_set(&i2c_imx->last_error, -EBUSY);
+			break;
+		case EVT_STOP:
+			imx_i2c_write_reg(i2c_imx->hwdata->i2sr_clr_opcode, i2c_imx,
+					IMX_I2C_I2SR);
+			imx_i2c_write_reg(i2c_imx->hwdata->i2cr_ien_opcode | I2CR_IIEN,
+					i2c_imx,IMX_I2C_I2CR);
+
+			i2c_imx->stopped = 1;
+			udelay(50);
+			set_state(i2c_imx, STATE_IDLE);
+			return;
+		case EVT_POLL:
+		default:
+			break;
+	}
+
+	wait_event_interruptible_timeout(i2c_imx->state_queue,(evt_is_empty(&i2c_imx->events) == 0), HZ / 10);
+}
+
+static void slave_evt_handler (struct imx_i2c_struct *i2c_imx, unsigned int event)
+{
+	u8 reg,data;
+	switch (event) {
+		case EVT_ENTRY:
+			if (atomic_read(&i2c_imx->last_error) == EUNDEFINED)
+			{
+				dev_dbg(&i2c_imx->adapter.dev, "Reset lost START event\n");
+				atomic_set(&i2c_imx->last_error, -EBUSY);
+			}
+			i2c_imx->start_retry_cnt = 0 ;
+			i2c_imx->slave_poll_cnt = 0 ;
+			return;
+		case EVT_AL:
+			set_state(i2c_imx, STATE_IDLE);
+			break;
+		case EVT_START:
+			reg = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
+			atomic_set(&i2c_imx->last_error, -EBUSY);
+			break;
+		case EVT_STOP:
+			break;
+		case EVT_SI:
+			i2c_imx_slave_process_irq(i2c_imx);
+			reg = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2SR);
+			if ((reg & I2SR_IBB) == 0) {
+				data = 0;
+				set_state(i2c_imx,  STATE_IDLE);
+				dev_dbg(&i2c_imx->adapter.dev, "end of package");
+				i2c_slave_event(i2c_imx->slave, I2C_SLAVE_STOP, &data);
+			}
+			if (i2c_imx->slave_poll_cnt > 10 )
+			{
+				dev_err(&i2c_imx->adapter.dev,"Too much slave loops (%i)\n",i2c_imx->slave_poll_cnt);
+			}
+
+			i2c_imx->slave_poll_cnt = 0 ;
+			break;
+		case EVT_POLL:
+			reg = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2SR);
+			if ((reg & I2SR_IBB) == 0) {
+				data = 0;
+				set_state(i2c_imx,  STATE_IDLE);
+				dev_dbg(&i2c_imx->adapter.dev, "end of package");
+				i2c_slave_event(i2c_imx->slave, I2C_SLAVE_STOP, &data);
+				if (i2c_imx->slave_poll_cnt > 10 )
+				{
+					dev_err(&i2c_imx->adapter.dev,"Too much slave loops (%i)\n",i2c_imx->slave_poll_cnt);
+				}
+
+				i2c_imx->slave_poll_cnt = 0 ;
+			}
+
+			/*TODO: do "dummy read" if no interrupts or stop condition for more then 10 wait loops*/
+			i2c_imx->slave_poll_cnt += 1 ;
+			if (i2c_imx->slave_poll_cnt % 10 == 0)
+			{
+				imx_i2c_read_reg(i2c_imx, IMX_I2C_I2DR);
+			}
+
+			break;
+		default:
+			break;
+	}
+
+	wait_event_interruptible_timeout(i2c_imx->state_queue,(evt_is_empty(&i2c_imx->events) == 0), 1);
+}
+
+static void sp_evt_handler (struct imx_i2c_struct *i2c_imx, unsigned int event)
+{
+	u8 reg;
+
+	switch (event) {
+		case EVT_AL:
+			dev_dbg(&i2c_imx->adapter.dev, "Lost arbitration on START");
+			atomic_set(&i2c_imx->last_error, -EAGAIN);
+			set_state(i2c_imx, STATE_IDLE);
+			return;
+		case EVT_SI:
+			set_state(i2c_imx, STATE_IDLE);
+			evt_put(&i2c_imx->events, EVT_SI );
+		case EVT_START:
+			atomic_set(&i2c_imx->last_error, -EBUSY);
+		case EVT_STOP:
+			break;
+
+		case EVT_ENTRY:
+			return;
+		case EVT_POLL:
+			reg = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2SR);
+			if ((reg & I2SR_IBB) && !(reg & I2SR_IAL)) {
+
+				set_state(i2c_imx,  STATE_MASTER);
+				reg = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
+
+				i2c_imx->stopped = 0;
+				reg |= I2CR_IIEN | I2CR_MTX | I2CR_TXAK;
+				reg &= ~I2CR_DMAEN;
+				imx_i2c_write_reg(reg, i2c_imx, IMX_I2C_I2CR);
+				atomic_set(&i2c_imx->last_error,0);
+				i2c_imx->start_retry_cnt = 0 ;
+				return;
+			}
+		default:
+			break;
+
+	}
+	if (i2c_imx->start_retry_cnt ++  < 100)
+	{
+		wait_event_interruptible_timeout(i2c_imx->state_queue,(evt_is_empty(&i2c_imx->events) == 0),1);
+		dev_dbg(&i2c_imx->adapter.dev, "wait for busy cnt = %i evt = %i", i2c_imx->start_retry_cnt,event);
+	} else {
+
+		dev_dbg(&i2c_imx->adapter.dev, "start timeout");
+		i2c_imx->start_retry_cnt = 0 ;
+		atomic_set(&i2c_imx->last_error,-ETIMEDOUT);
+		set_state(i2c_imx, STATE_IDLE);
+		wait_event_interruptible_timeout(i2c_imx->state_queue,(evt_is_empty(&i2c_imx->events) == 0), HZ / 10);
+	}
+}
+
+static void set_state (struct imx_i2c_struct *i2c_imx, unsigned int new)
+{
+	i2c_imx->state = new;
+	switch (new) {
+		case STATE_IDLE:
+			idle_evt_handler(i2c_imx, EVT_ENTRY);
+			break;
+		case STATE_SLAVE:
+			slave_evt_handler(i2c_imx, EVT_ENTRY);
+			break;
+		case STATE_SP:
+			sp_evt_handler(i2c_imx, EVT_ENTRY);
+			break;
+		case STATE_MASTER:
+			master_evt_handler(i2c_imx, EVT_ENTRY);
+			break;
+	}
+}
+
+
+static int i2c_imx_slave_threadfn(void *pdata)
+{
+	unsigned int event;
+	struct imx_i2c_struct *i2c_imx = (struct imx_i2c_struct *) pdata;
+
+	do {
+
+		event = evt_get(&i2c_imx->events);
+		if (event == EVT_INVALID)
+			event = EVT_POLL;
+
+		switch (i2c_imx->state) {
+			case STATE_IDLE:
+				idle_evt_handler(i2c_imx, event);
+				break;
+			case STATE_SLAVE:
+				slave_evt_handler(i2c_imx, event);
+				break;
+			case STATE_SP:
+				sp_evt_handler(i2c_imx, event);
+				break;
+			case STATE_MASTER:
+				master_evt_handler(i2c_imx, event);
+				break;
+			default:
+				break;
+
+		}
+
+	} while (kthread_should_stop() == 0);
+
+	return 0;
 }
 
 static irqreturn_t i2c_imx_isr(int irq, void *dev_id)
 {
 	struct imx_i2c_struct *i2c_imx = dev_id;
-	unsigned int temp;
+	unsigned int status, ctl;
+
+	status = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2SR);
+	ctl = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2CR);
+	if (status & I2SR_IIF) {
+		i2c_imx_clear_isr_bit(i2c_imx, status);
+
+		if (ctl & I2CR_MSTA) {
+			dev_dbg(&i2c_imx->adapter.dev, "master interrupt");
+			i2c_imx->master_interrupt = 1;
+			wake_up(&i2c_imx->queue);
+		} else 	if (status & I2SR_IAL) {
+			evt_put(&i2c_imx->events, EVT_AL );
+		} else  {
+			dev_dbg(&i2c_imx->adapter.dev, "slave interrupt");
+			evt_put(&i2c_imx->events, EVT_SI );
+			wake_up(&i2c_imx->state_queue);
+		}
 
-	temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2SR);
-	if (temp & I2SR_IIF) {
-		/* save status register */
-		i2c_imx->i2csr = temp;
-		temp &= ~I2SR_IIF;
-		temp |= (i2c_imx->hwdata->i2sr_clr_opcode & I2SR_IIF);
-		imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2SR);
-		wake_up(&i2c_imx->queue);
 		return IRQ_HANDLED;
 	}
 
@@ -895,7 +1444,13 @@  static int i2c_imx_xfer(struct i2c_adapter *adapter,
 
 	/* Start I2C transfer */
 	result = i2c_imx_start(i2c_imx);
-	if (result) {
+	if (result == -ETIMEDOUT) {
+		/*
+			Recovery is not started on arbitration lost, since it can break
+			slave transfer. But in case of "bus timeout" recovery it
+			could be useful to bring controller out of "strange state".
+		*/
+		dev_dbg(&i2c_imx->adapter.dev, "call bus recovery");
 		if (i2c_imx->adapter.bus_recovery_info) {
 			i2c_recover_bus(&i2c_imx->adapter);
 			result = i2c_imx_start(i2c_imx);
@@ -1024,6 +1579,60 @@  static int i2c_imx_init_recovery_info(struct imx_i2c_struct *i2c_imx,
 	rinfo->unprepare_recovery = i2c_imx_unprepare_recovery;
 	rinfo->recover_bus = i2c_generic_gpio_recovery;
 	i2c_imx->adapter.bus_recovery_info = rinfo;
+}
+
+static int i2c_imx_reg_slave(struct i2c_client *slave)
+{
+	struct imx_i2c_struct *i2c_imx = i2c_get_adapdata(slave->adapter);
+	int result;
+	struct sched_param param = { .sched_priority = MAX_RT_PRIO - 1 };
+
+	dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
+
+	if (i2c_imx->slave)
+		return -EBUSY;
+
+	if (slave->flags & I2C_CLIENT_TEN)
+		return -EAFNOSUPPORT;
+
+	i2c_imx->slave = slave;
+
+	/* Set the Slave address */
+	imx_i2c_write_reg((i2c_imx->slave->addr << 1), i2c_imx, IMX_I2C_IADR);
+
+	result = i2c_imx_hw_start(i2c_imx);
+	if (result != 0)
+		return result;
+
+	i2c_imx->slave_task = kthread_run(i2c_imx_slave_threadfn,
+		(void *) i2c_imx, "i2c-slave-%s", i2c_imx->adapter.name);
+
+	sched_setscheduler(i2c_imx->slave_task, SCHED_FIFO, &param);
+
+	if (IS_ERR(i2c_imx->slave_task))
+		return PTR_ERR(i2c_imx->slave_task);
+
+	i2c_imx_slave_init(i2c_imx);
+
+	return 0;
+
+}
+
+static int i2c_imx_unreg_slave(struct i2c_client *slave)
+{
+	struct imx_i2c_struct *i2c_imx = i2c_get_adapdata(slave->adapter);
+
+	dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
+	if (i2c_imx->slave_task != NULL)
+		kthread_stop(i2c_imx->slave_task);
+
+	wake_up(&i2c_imx->state_queue);
+
+	i2c_imx->slave_task = NULL;
+
+	i2c_imx->slave = NULL;
+
+	i2c_imx_stop(i2c_imx);
 
 	return 0;
 }
@@ -1037,6 +1646,8 @@  static u32 i2c_imx_func(struct i2c_adapter *adapter)
 static struct i2c_algorithm i2c_imx_algo = {
 	.master_xfer	= i2c_imx_xfer,
 	.functionality	= i2c_imx_func,
+	.reg_slave		= i2c_imx_reg_slave,
+	.unreg_slave	= i2c_imx_unreg_slave,
 };
 
 static int i2c_imx_probe(struct platform_device *pdev)
@@ -1096,6 +1707,12 @@  static int i2c_imx_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	i2c_imx->pinctrl = devm_pinctrl_get(&pdev->dev);
+	if (IS_ERR(i2c_imx->pinctrl)) {
+		ret = PTR_ERR(i2c_imx->pinctrl);
+		goto clk_disable;
+	}
+
 	/* Request IRQ */
 	ret = devm_request_irq(&pdev->dev, irq, i2c_imx_isr, 0,
 				pdev->name, i2c_imx);
@@ -1106,6 +1723,7 @@  static int i2c_imx_probe(struct platform_device *pdev)
 
 	/* Init queue */
 	init_waitqueue_head(&i2c_imx->queue);
+	init_waitqueue_head(&i2c_imx->state_queue);
 
 	/* Set up adapter data */
 	i2c_set_adapdata(&i2c_imx->adapter, i2c_imx);
@@ -1157,6 +1775,9 @@  static int i2c_imx_probe(struct platform_device *pdev)
 	/* Init DMA config if supported */
 	i2c_imx_dma_request(i2c_imx, phy_addr);
 
+	/* init slave_state to IDLE */
+	i2c_imx->state = STATE_IDLE;
+	evt_init(&i2c_imx->events);
 	return 0;   /* Return OK */
 
 rpm_disable:
@@ -1183,6 +1804,9 @@  static int i2c_imx_remove(struct platform_device *pdev)
 	dev_dbg(&i2c_imx->adapter.dev, "adapter removed\n");
 	i2c_del_adapter(&i2c_imx->adapter);
 
+	if (i2c_imx->slave_task != NULL)
+		kthread_stop(i2c_imx->slave_task);
+
 	if (i2c_imx->dma)
 		i2c_imx_dma_free(i2c_imx);