@@ -136,6 +136,8 @@ struct fsi_i2c_master {
u8 fifo_size;
struct list_head ports;
struct ida ida;
+ wait_queue_head_t wait;
+ struct semaphore lock;
};
struct fsi_i2c_port {
@@ -171,6 +173,29 @@ static int fsi_i2c_write_reg(struct fsi_device *fsi, unsigned int reg,
return fsi_device_write(fsi, reg, &data_be, sizeof(data_be));
}
+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 int fsi_i2c_dev_init(struct fsi_i2c_master *i2c)
{
int rc;
@@ -412,25 +437,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)
@@ -455,6 +486,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->idx = ida_simple_get(&fsi_i2c_ida, 1, INT_MAX, GFP_KERNEL);
ida_init(&i2c->ida);