@@ -135,6 +135,8 @@ struct fsi_i2c_master {
u8 fifo_size;
unsigned long clk;
struct list_head ports;
+ wait_queue_head_t wait;
+ struct semaphore lock;
};
struct fsi_i2c_port {
@@ -145,6 +147,29 @@ struct fsi_i2c_port {
u16 xfrd;
};
+static int fsi_i2c_lock_master(struct fsi_i2c_master *i2c, int timeout)
+{
+ int rc;
+
+ rc = down_trylock(&i2c->lock);
+ if (!rc)
+ return 0;
+
+ rc = wait_event_interruptible_timeout(i2c->wait,
+ !down_trylock(&i2c->lock),
+ timeout);
+ if (rc > 0)
+ return 0;
+
+ return -EBUSY;
+}
+
+static void fsi_i2c_unlock_master(struct fsi_i2c_master *i2c)
+{
+ up(&i2c->lock);
+ wake_up(&i2c->wait);
+}
+
static unsigned long fsi_i2c_get_clk_div(struct fsi_i2c_master *i2c)
{
unsigned long clk_div = I2C_DEFAULT_CLK_DIV;
@@ -400,25 +425,31 @@ static int fsi_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
struct fsi_i2c_port *port = adap->algo_data;
struct i2c_msg *msg;
- rc = fsi_i2c_set_port(port);
+ rc = fsi_i2c_lock_master(port->master, adap->timeout);
if (rc)
return rc;
+ rc = fsi_i2c_set_port(port);
+ if (rc)
+ goto unlock;
+
for (i = 0; i < num; ++i) {
msg = msgs + i;
start_time = jiffies;
rc = fsi_i2c_start(port, msg, i == num - 1);
if (rc)
- return rc;
+ goto unlock;
rc = fsi_i2c_wait(port, msg,
adap->timeout - (jiffies - start_time));
if (rc)
- return rc;
+ goto unlock;
}
- return 0;
+unlock:
+ fsi_i2c_unlock_master(port->master);
+ return rc;
}
static u32 fsi_i2c_functionality(struct i2c_adapter *adap)
@@ -448,6 +479,8 @@ static int fsi_i2c_probe(struct device *dev)
if (!i2c)
return -ENOMEM;
+ init_waitqueue_head(&i2c->wait);
+ sema_init(&i2c->lock, 1);
i2c->fsi = to_fsi_dev(dev);
i2c->clk = I2C_DEFAULT_CLK;
INIT_LIST_HEAD(&i2c->ports);