1 files changed, 392 insertions, 16 deletions

diff --git a/drivers/iio/accel/bmc150-accel.c b/drivers/iio/accel/bmc150-accel.c
index f1b80c4c0b35..4026122a7592 100644
--- a/drivers/iio/accel/bmc150-accel.c
+++ b/drivers/iio/accel/bmc150-accel.c
@@ -70,7 +70,9 @@
 #define BMC150_ACCEL_INT_MAP_0_BIT_SLOPE        BIT(2)
 
 #define BMC150_ACCEL_REG_INT_MAP_1              0x1A
-#define BMC150_ACCEL_INT_MAP_1_BIT_DATA BIT(0)
+#define BMC150_ACCEL_INT_MAP_1_BIT_DATA         BIT(0)
+#define BMC150_ACCEL_INT_MAP_1_BIT_FWM          BIT(1)
+#define BMC150_ACCEL_INT_MAP_1_BIT_FFULL        BIT(2)
 
 #define BMC150_ACCEL_REG_INT_RST_LATCH          0x21
 #define BMC150_ACCEL_INT_MODE_LATCH_RESET       0x80
@@ -83,7 +85,9 @@
 #define BMC150_ACCEL_INT_EN_BIT_SLP_Z           BIT(2)
 
 #define BMC150_ACCEL_REG_INT_EN_1               0x17
-#define BMC150_ACCEL_INT_EN_BIT_DATA_EN BIT(4)
+#define BMC150_ACCEL_INT_EN_BIT_DATA_EN         BIT(4)
+#define BMC150_ACCEL_INT_EN_BIT_FFULL_EN        BIT(5)
+#define BMC150_ACCEL_INT_EN_BIT_FWM_EN          BIT(6)
 
 #define BMC150_ACCEL_REG_INT_OUT_CTRL           0x20
 #define BMC150_ACCEL_INT_OUT_CTRL_INT1_LVL      BIT(0)
@@ -122,6 +126,12 @@
 #define BMC150_ACCEL_AXIS_TO_REG(axis)  (BMC150_ACCEL_REG_XOUT_L + (axis * 2))
 #define BMC150_AUTO_SUSPEND_DELAY_MS            2000
 
+#define BMC150_ACCEL_REG_FIFO_STATUS            0x0E
+#define BMC150_ACCEL_REG_FIFO_CONFIG0           0x30
+#define BMC150_ACCEL_REG_FIFO_CONFIG1           0x3E
+#define BMC150_ACCEL_REG_FIFO_DATA              0x3F
+#define BMC150_ACCEL_FIFO_LENGTH                32
+
 enum bmc150_accel_axis {
         AXIS_X,
         AXIS_Y,
@@ -179,13 +189,14 @@ struct bmc150_accel_data {
         atomic_t active_intr;
         struct bmc150_accel_trigger triggers[BMC150_ACCEL_TRIGGERS];
         struct mutex mutex;
+        u8 fifo_mode, watermark;
         s16 buffer[8];
         u8 bw_bits;
         u32 slope_dur;
         u32 slope_thres;
         u32 range;
         int ev_enable_state;
-        int64_t timestamp;
+        int64_t timestamp, old_timestamp;
         const struct bmc150_accel_chip_info *chip_info;
 };
 
@@ -470,6 +481,12 @@ static const struct bmc150_accel_interrupt_info {
                         BMC150_ACCEL_INT_EN_BIT_SLP_Y |
                         BMC150_ACCEL_INT_EN_BIT_SLP_Z
         },
+        { /* fifo watermark interrupt */
+                .map_reg = BMC150_ACCEL_REG_INT_MAP_1,
+                .map_bitmask = BMC150_ACCEL_INT_MAP_1_BIT_FWM,
+                .en_reg = BMC150_ACCEL_REG_INT_EN_1,
+                .en_bitmask = BMC150_ACCEL_INT_EN_BIT_FWM_EN,
+        },
 };
 
 static void bmc150_accel_interrupts_setup(struct iio_dev *indio_dev,
@@ -823,6 +840,214 @@ static int bmc150_accel_validate_trigger(struct iio_dev *indio_dev,
         return -EINVAL;
 }
 
+static ssize_t bmc150_accel_get_fifo_watermark(struct device *dev,
+                                               struct device_attribute *attr,
+                                               char *buf)
+{
+        struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+        struct bmc150_accel_data *data = iio_priv(indio_dev);
+        int wm;
+
+        mutex_lock(&data->mutex);
+        wm = data->watermark;
+        mutex_unlock(&data->mutex);
+
+        return sprintf(buf, "%d\n", wm);
+}
+
+static ssize_t bmc150_accel_get_fifo_state(struct device *dev,
+                                           struct device_attribute *attr,
+                                           char *buf)
+{
+        struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+        struct bmc150_accel_data *data = iio_priv(indio_dev);
+        bool state;
+
+        mutex_lock(&data->mutex);
+        state = data->fifo_mode;
+        mutex_unlock(&data->mutex);
+
+        return sprintf(buf, "%d\n", state);
+}
+
+static IIO_CONST_ATTR(hwfifo_watermark_min, "1");
+static IIO_CONST_ATTR(hwfifo_watermark_max,
+                      __stringify(BMC150_ACCEL_FIFO_LENGTH));
+static IIO_DEVICE_ATTR(hwfifo_enabled, S_IRUGO,
+                       bmc150_accel_get_fifo_state, NULL, 0);
+static IIO_DEVICE_ATTR(hwfifo_watermark, S_IRUGO,
+                       bmc150_accel_get_fifo_watermark, NULL, 0);
+
+static const struct attribute *bmc150_accel_fifo_attributes[] = {
+        &iio_const_attr_hwfifo_watermark_min.dev_attr.attr,
+        &iio_const_attr_hwfifo_watermark_max.dev_attr.attr,
+        &iio_dev_attr_hwfifo_watermark.dev_attr.attr,
+        &iio_dev_attr_hwfifo_enabled.dev_attr.attr,
+        NULL,
+};
+
+static int bmc150_accel_set_watermark(struct iio_dev *indio_dev, unsigned val)
+{
+        struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+        if (val > BMC150_ACCEL_FIFO_LENGTH)
+                val = BMC150_ACCEL_FIFO_LENGTH;
+
+        mutex_lock(&data->mutex);
+        data->watermark = val;
+        mutex_unlock(&data->mutex);
+
+        return 0;
+}
+
+/*
+ * We must read at least one full frame in one burst, otherwise the rest of the
+ * frame data is discarded.
+ */
+static int bmc150_accel_fifo_transfer(const struct i2c_client *client,
+                                      char *buffer, int samples)
+{
+        int sample_length = 3 * 2;
+        u8 reg_fifo_data = BMC150_ACCEL_REG_FIFO_DATA;
+        int ret = -EIO;
+
+        if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                struct i2c_msg msg[2] = {
+                        {
+                                .addr = client->addr,
+                                .flags = 0,
+                                .buf = &reg_fifo_data,
+                                .len = sizeof(reg_fifo_data),
+                        },
+                        {
+                                .addr = client->addr,
+                                .flags = I2C_M_RD,
+                                .buf = (u8 *)buffer,
+                                .len = samples * sample_length,
+                        }
+                };
+
+                ret = i2c_transfer(client->adapter, msg, 2);
+                if (ret != 2)
+                        ret = -EIO;
+                else
+                        ret = 0;
+        } else {
+                int i, step = I2C_SMBUS_BLOCK_MAX / sample_length;
+
+                for (i = 0; i < samples * sample_length; i += step) {
+                        ret = i2c_smbus_read_i2c_block_data(client,
+                                                            reg_fifo_data, step,
+                                                            &buffer[i]);
+                        if (ret != step) {
+                                ret = -EIO;
+                                break;
+                        }
+
+                        ret = 0;
+                }
+        }
+
+        if (ret)
+                dev_err(&client->dev, "Error transferring data from fifo\n");
+
+        return ret;
+}
+
+static int __bmc150_accel_fifo_flush(struct iio_dev *indio_dev,
+                                     unsigned samples, bool irq)
+{
+        struct bmc150_accel_data *data = iio_priv(indio_dev);
+        int ret, i;
+        u8 count;
+        u16 buffer[BMC150_ACCEL_FIFO_LENGTH * 3];
+        int64_t tstamp;
+        uint64_t sample_period;
+        ret = i2c_smbus_read_byte_data(data->client,
+                                       BMC150_ACCEL_REG_FIFO_STATUS);
+        if (ret < 0) {
+                dev_err(&data->client->dev, "Error reading reg_fifo_status\n");
+                return ret;
+        }
+
+        count = ret & 0x7F;
+
+        if (!count)
+                return 0;
+
+        /*
+         * If we getting called from IRQ handler we know the stored timestamp is
+         * fairly accurate for the last stored sample. Otherwise, if we are
+         * called as a result of a read operation from userspace and hence
+         * before the watermark interrupt was triggered, take a timestamp
+         * now. We can fall anywhere in between two samples so the error in this
+         * case is at most one sample period.
+         */
+        if (!irq) {
+                data->old_timestamp = data->timestamp;
+                data->timestamp = iio_get_time_ns();
+        }
+
+        /*
+         * Approximate timestamps for each of the sample based on the sampling
+         * frequency, timestamp for last sample and number of samples.
+         *
+         * Note that we can't use the current bandwidth settings to compute the
+         * sample period because the sample rate varies with the device
+         * (e.g. between 31.70ms to 32.20ms for a bandwidth of 15.63HZ). That
+         * small variation adds when we store a large number of samples and
+         * creates significant jitter between the last and first samples in
+         * different batches (e.g. 32ms vs 21ms).
+         *
+         * To avoid this issue we compute the actual sample period ourselves
+         * based on the timestamp delta between the last two flush operations.
+         */
+        sample_period = (data->timestamp - data->old_timestamp);
+        do_div(sample_period, count);
+        tstamp = data->timestamp - (count - 1) * sample_period;
+
+        if (samples && count > samples)
+                count = samples;
+
+        ret = bmc150_accel_fifo_transfer(data->client, (u8 *)buffer, count);
+        if (ret)
+                return ret;
+
+        /*
+         * Ideally we want the IIO core to handle the demux when running in fifo
+         * mode but not when running in triggered buffer mode. Unfortunately
+         * this does not seem to be possible, so stick with driver demux for
+         * now.
+         */
+        for (i = 0; i < count; i++) {
+                u16 sample[8];
+                int j, bit;
+
+                j = 0;
+                for_each_set_bit(bit, indio_dev->active_scan_mask,
+                                 indio_dev->masklength)
+                        memcpy(&sample[j++], &buffer[i * 3 + bit], 2);
+
+                iio_push_to_buffers_with_timestamp(indio_dev, sample, tstamp);
+
+                tstamp += sample_period;
+        }
+
+        return count;
+}
+
+static int bmc150_accel_fifo_flush(struct iio_dev *indio_dev, unsigned samples)
+{
+        struct bmc150_accel_data *data = iio_priv(indio_dev);
+        int ret;
+
+        mutex_lock(&data->mutex);
+        ret = __bmc150_accel_fifo_flush(indio_dev, samples, false);
+        mutex_unlock(&data->mutex);
+
+        return ret;
+}
+
 static IIO_CONST_ATTR_SAMP_FREQ_AVAIL(
                 "7.810000 15.630000 31.250000 62.500000 125 250 500 1000");
 
@@ -962,6 +1187,20 @@ static const struct iio_info bmc150_accel_info = {
         .driver_module          = THIS_MODULE,
 };
 
+static const struct iio_info bmc150_accel_info_fifo = {
+        .attrs                  = &bmc150_accel_attrs_group,
+        .read_raw               = bmc150_accel_read_raw,
+        .write_raw              = bmc150_accel_write_raw,
+        .read_event_value       = bmc150_accel_read_event,
+        .write_event_value      = bmc150_accel_write_event,
+        .write_event_config     = bmc150_accel_write_event_config,
+        .read_event_config      = bmc150_accel_read_event_config,
+        .validate_trigger       = bmc150_accel_validate_trigger,
+        .hwfifo_set_watermark   = bmc150_accel_set_watermark,
+        .hwfifo_flush_to_buffer = bmc150_accel_fifo_flush,
+        .driver_module          = THIS_MODULE,
+};
+
 static irqreturn_t bmc150_accel_trigger_handler(int irq, void *p)
 {
         struct iio_poll_func *pf = p;
@@ -1057,18 +1296,17 @@ static const struct iio_trigger_ops bmc150_accel_trigger_ops = {
         .owner = THIS_MODULE,
 };
 
-static irqreturn_t bmc150_accel_event_handler(int irq, void *private)
+static int bmc150_accel_handle_roc_event(struct iio_dev *indio_dev)
 {
-        struct iio_dev *indio_dev = private;
         struct bmc150_accel_data *data = iio_priv(indio_dev);
-        int ret;
         int dir;
+        int ret;
 
         ret = i2c_smbus_read_byte_data(data->client,
                                        BMC150_ACCEL_REG_INT_STATUS_2);
         if (ret < 0) {
                 dev_err(&data->client->dev, "Error reading reg_int_status_2\n");
-                goto ack_intr_status;
+                return ret;
         }
 
         if (ret & BMC150_ACCEL_ANY_MOTION_BIT_SIGN)
@@ -1097,35 +1335,73 @@ static irqreturn_t bmc150_accel_event_handler(int irq, void *private)
                                                         IIO_EV_TYPE_ROC,
                                                         dir),
                                                         data->timestamp);
-ack_intr_status:
-        if (!data->triggers[BMC150_ACCEL_TRIGGER_DATA_READY].enabled)
+        return ret;
+}
+
+static irqreturn_t bmc150_accel_irq_thread_handler(int irq, void *private)
+{
+        struct iio_dev *indio_dev = private;
+        struct bmc150_accel_data *data = iio_priv(indio_dev);
+        bool ack = false;
+        int ret;
+
+        mutex_lock(&data->mutex);
+
+        if (data->fifo_mode) {
+                ret = __bmc150_accel_fifo_flush(indio_dev,
+                                                BMC150_ACCEL_FIFO_LENGTH, true);
+                if (ret > 0)
+                        ack = true;
+        }
+
+        if (data->ev_enable_state) {
+                ret = bmc150_accel_handle_roc_event(indio_dev);
+                if (ret > 0)
+                        ack = true;
+        }
+
+        if (ack) {
                 ret = i2c_smbus_write_byte_data(data->client,
                                         BMC150_ACCEL_REG_INT_RST_LATCH,
                                         BMC150_ACCEL_INT_MODE_LATCH_INT |
                                         BMC150_ACCEL_INT_MODE_LATCH_RESET);
+                if (ret)
+                        dev_err(&data->client->dev, "Error writing reg_int_rst_latch\n");
+                ret = IRQ_HANDLED;
+        } else {
+                ret = IRQ_NONE;
+        }
 
-        return IRQ_HANDLED;
+        mutex_unlock(&data->mutex);
+
+        return ret;
 }
 
-static irqreturn_t bmc150_accel_data_rdy_trig_poll(int irq, void *private)
+static irqreturn_t bmc150_accel_irq_handler(int irq, void *private)
 {
         struct iio_dev *indio_dev = private;
         struct bmc150_accel_data *data = iio_priv(indio_dev);
+        bool ack = false;
         int i;
 
+        data->old_timestamp = data->timestamp;
         data->timestamp = iio_get_time_ns();
 
         for (i = 0; i < BMC150_ACCEL_TRIGGERS; i++) {
                 if (data->triggers[i].enabled) {
                         iio_trigger_poll(data->triggers[i].indio_trig);
+                        ack = true;
                         break;
                 }
         }
 
-        if (data->ev_enable_state)
+        if (data->ev_enable_state || data->fifo_mode)
                 return IRQ_WAKE_THREAD;
-        else
+
+        if (ack)
                 return IRQ_HANDLED;
+
+        return IRQ_NONE;
 }
 
 static const char *bmc150_accel_match_acpi_device(struct device *dev, int *data)
@@ -1232,6 +1508,94 @@ static int bmc150_accel_triggers_setup(struct iio_dev *indio_dev,
         return ret;
 }
 
+#define BMC150_ACCEL_FIFO_MODE_STREAM          0x80
+#define BMC150_ACCEL_FIFO_MODE_FIFO            0x40
+#define BMC150_ACCEL_FIFO_MODE_BYPASS          0x00
+
+static int bmc150_accel_fifo_set_mode(struct bmc150_accel_data *data)
+{
+        u8 reg = BMC150_ACCEL_REG_FIFO_CONFIG1;
+        int ret;
+
+        ret = i2c_smbus_write_byte_data(data->client, reg, data->fifo_mode);
+        if (ret < 0) {
+                dev_err(&data->client->dev, "Error writing reg_fifo_config1\n");
+                return ret;
+        }
+
+        if (!data->fifo_mode)
+                return 0;
+
+        ret = i2c_smbus_write_byte_data(data->client,
+                                        BMC150_ACCEL_REG_FIFO_CONFIG0,
+                                        data->watermark);
+        if (ret < 0)
+                dev_err(&data->client->dev, "Error writing reg_fifo_config0\n");
+
+        return ret;
+}
+
+static int bmc150_accel_buffer_postenable(struct iio_dev *indio_dev)
+{
+        struct bmc150_accel_data *data = iio_priv(indio_dev);
+        int ret = 0;
+
+        if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED)
+                return iio_triggered_buffer_postenable(indio_dev);
+
+        mutex_lock(&data->mutex);
+
+        if (!data->watermark)
+                goto out;
+
+        ret = bmc150_accel_set_interrupt(data, BMC150_ACCEL_INT_WATERMARK,
+                                         true);
+        if (ret)
+                goto out;
+
+        data->fifo_mode = BMC150_ACCEL_FIFO_MODE_FIFO;
+
+        ret = bmc150_accel_fifo_set_mode(data);
+        if (ret) {
+                data->fifo_mode = 0;
+                bmc150_accel_set_interrupt(data, BMC150_ACCEL_INT_WATERMARK,
+                                           false);
+        }
+
+out:
+        mutex_unlock(&data->mutex);
+
+        return ret;
+}
+
+static int bmc150_accel_buffer_predisable(struct iio_dev *indio_dev)
+{
+        struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+        if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED)
+                return iio_triggered_buffer_predisable(indio_dev);
+
+        mutex_lock(&data->mutex);
+
+        if (!data->fifo_mode)
+                goto out;
+
+        bmc150_accel_set_interrupt(data, BMC150_ACCEL_INT_WATERMARK, false);
+        __bmc150_accel_fifo_flush(indio_dev, BMC150_ACCEL_FIFO_LENGTH, false);
+        data->fifo_mode = 0;
+        bmc150_accel_fifo_set_mode(data);
+
+out:
+        mutex_unlock(&data->mutex);
+
+        return 0;
+}
+
+static const struct iio_buffer_setup_ops bmc150_accel_buffer_ops = {
+        .postenable = bmc150_accel_buffer_postenable,
+        .predisable = bmc150_accel_buffer_predisable,
+};
+
 static int bmc150_accel_probe(struct i2c_client *client,
                               const struct i2c_device_id *id)
 {
@@ -1278,8 +1642,8 @@ static int bmc150_accel_probe(struct i2c_client *client,
         if (client->irq >= 0) {
                 ret = devm_request_threaded_irq(
                                                 &client->dev, client->irq,
-                                                bmc150_accel_data_rdy_trig_poll,
-                                                bmc150_accel_event_handler,
+                                                bmc150_accel_irq_handler,
+                                                bmc150_accel_irq_thread_handler,
                                                 IRQF_TRIGGER_RISING,
                                                 BMC150_ACCEL_IRQ_NAME,
                                                 indio_dev);
@@ -1309,12 +1673,20 @@ static int bmc150_accel_probe(struct i2c_client *client,
                 ret = iio_triggered_buffer_setup(indio_dev,
                                                  &iio_pollfunc_store_time,
                                                  bmc150_accel_trigger_handler,
-                                                 NULL);
+                                                 &bmc150_accel_buffer_ops);
                 if (ret < 0) {
                         dev_err(&client->dev,
                                 "Failed: iio triggered buffer setup\n");
                         goto err_trigger_unregister;
                 }
+
+                if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C) ||
+                    i2c_check_functionality(client->adapter,
+                                            I2C_FUNC_SMBUS_READ_I2C_BLOCK)) {
+                        indio_dev->modes |= INDIO_BUFFER_SOFTWARE;
+                        indio_dev->info = &bmc150_accel_info_fifo;
+                        indio_dev->buffer->attrs = bmc150_accel_fifo_attributes;
+                }
         }
 
         ret = iio_device_register(indio_dev);
@@ -1386,6 +1758,7 @@ static int bmc150_accel_resume(struct device *dev)
         mutex_lock(&data->mutex);
         if (atomic_read(&data->active_intr))
                 bmc150_accel_set_mode(data, BMC150_ACCEL_SLEEP_MODE_NORMAL, 0);
+        bmc150_accel_fifo_set_mode(data);
         mutex_unlock(&data->mutex);
 
         return 0;
@@ -1419,6 +1792,9 @@ static int bmc150_accel_runtime_resume(struct device *dev)
         ret = bmc150_accel_set_mode(data, BMC150_ACCEL_SLEEP_MODE_NORMAL, 0);
         if (ret < 0)
                 return ret;
+        ret = bmc150_accel_fifo_set_mode(data);
+        if (ret < 0)
+                return ret;
 
         sleep_val = bmc150_accel_get_startup_times(data);
         if (sleep_val < 20)