i2c: imx: add slave support. v2 status

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