Re: [PATCH] i2c: imx: add slave support. v2 status

[Maxim Syrchin <syrchin@dev.rtsoft.ru>]

[-- Attachment #1: Type: text/plain, Size: 1592 bytes --]

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
>>


[-- Attachment #2: 0001-i2c-imx-add-slave-support.-v3.patch --]
[-- Type: text/plain, Size: 22747 bytes --]

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(-)

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);
 
-- 
2.1.4