[PATCH v2 11/11] iio: bmc150: add support for hardware fifo

[Octavian Purdila <octavian.purdila@intel.com>]

Add a new watermark trigger and hardware fifo operations. When the
watermark trigger is activated the watermark level is set and the
hardware FIFO is activated.

Signed-off-by: Octavian Purdila <octavian.purdila@intel.com>
---
 drivers/iio/accel/bmc150-accel.c | 194 ++++++++++++++++++++++++++++++++++++++-
 1 file changed, 190 insertions(+), 4 deletions(-)

diff --git a/drivers/iio/accel/bmc150-accel.c b/drivers/iio/accel/bmc150-accel.c
index 14509be..0aa3126 100644
--- a/drivers/iio/accel/bmc150-accel.c
+++ b/drivers/iio/accel/bmc150-accel.c
@@ -67,7 +67,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
@@ -80,7 +82,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)
@@ -119,6 +123,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,
@@ -161,6 +171,7 @@ struct bmc150_accel_trigger {
 	struct bmc150_accel_data *data;
 	struct iio_trigger *indio_trig;
 	bool enabled;
+	int (*setup)(struct bmc150_accel_trigger *t, bool state);
 };
 
 struct bmc150_accel_event {
@@ -180,8 +191,8 @@ struct bmc150_accel_event {
 	};
 };
 
-#define BMC150_ACCEL_INTERRUPTS		2
-#define BMC150_ACCEL_TRIGGERS		2
+#define BMC150_ACCEL_INTERRUPTS		3
+#define BMC150_ACCEL_TRIGGERS		3
 #define BMC150_ACCEL_EVENTS		1
 
 struct bmc150_accel_data {
@@ -191,6 +202,7 @@ struct bmc150_accel_data {
 	struct bmc150_accel_trigger triggers[BMC150_ACCEL_TRIGGERS];
 	struct bmc150_accel_event events[BMC150_ACCEL_EVENTS];
 	struct mutex mutex;
+	u8 fifo_mode, watermark;
 	s16 buffer[8];
 	u8 bw_bits;
 	u32 range;
@@ -484,6 +496,12 @@ bmc150_accel_interrupts[BMC150_ACCEL_INTERRUPTS] = {
 			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,
@@ -1020,6 +1038,8 @@ static const struct iio_info bmc150_accel_info = {
 	.driver_module		= THIS_MODULE,
 };
 
+static int bmc150_accel_fifo_flush(struct iio_dev *indio_dev);
+
 static irqreturn_t bmc150_accel_trigger_handler(int irq, void *p)
 {
 	struct iio_poll_func *pf = p;
@@ -1027,6 +1047,12 @@ static irqreturn_t bmc150_accel_trigger_handler(int irq, void *p)
 	struct bmc150_accel_data *data = iio_priv(indio_dev);
 	int bit, ret, i = 0;
 
+	if (data->fifo_mode) {
+		bmc150_accel_fifo_flush(indio_dev);
+		iio_trigger_notify_done(indio_dev->trig);
+		return IRQ_HANDLED;
+	}
+
 	mutex_lock(&data->mutex);
 	for_each_set_bit(bit, indio_dev->buffer->scan_mask,
 			 indio_dev->masklength) {
@@ -1088,6 +1114,14 @@ static int bmc150_accel_data_rdy_trigger_set_state(struct iio_trigger *trig,
 		return 0;
 	}
 
+	if (t->setup) {
+		ret = t->setup(t, state);
+		if (ret < 0) {
+			mutex_unlock(&data->mutex);
+			return ret;
+		}
+	}
+
 	ret = bmc150_accel_set_interrupt(data, t->intr, state);
 	if (ret < 0) {
 		mutex_unlock(&data->mutex);
@@ -1208,9 +1242,12 @@ static int bmc150_accel_gpio_probe(struct i2c_client *client,
 	return ret;
 }
 
+static int bmc150_accel_fifo_setup(struct bmc150_accel_trigger *t, bool state);
+
 static struct {
 	int intr;
 	const char *name;
+	int (*setup)(struct bmc150_accel_trigger *t, bool state);
 } bmc150_accel_triggers[BMC150_ACCEL_TRIGGERS] = {
 	{
 		.intr = 0,
@@ -1220,6 +1257,11 @@ static struct {
 		.intr = 1,
 		.name = "%s-any-motion-dev%d",
 	},
+	{
+		.intr = 2,
+		.name = "%s-watermark-dev%d",
+		.setup = bmc150_accel_fifo_setup,
+	},
 };
 
 static void bmc150_accel_unregister_triggers(struct bmc150_accel_data *data,
@@ -1257,6 +1299,7 @@ static int bmc150_accel_triggers_setup(struct iio_dev *indio_dev,
 		t->indio_trig->ops = &bmc150_accel_trigger_ops;
 		t->intr = &data->interrupts[intr];
 		t->data = data;
+		t->setup = bmc150_accel_triggers[i].setup;
 		iio_trigger_set_drvdata(t->indio_trig, t);
 
 		ret = iio_trigger_register(t->indio_trig);
@@ -1302,6 +1345,143 @@ static void bmc150_accel_events_setup(struct iio_dev *indio_dev,
 	}
 }
 
+static int bmc150_accel_set_watermark(struct iio_dev *indio_dev, unsigned val)
+
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	u8 reg = BMC150_ACCEL_REG_FIFO_CONFIG0;
+	int ret;
+
+	if (val > BMC150_ACCEL_FIFO_LENGTH)
+		return -EINVAL;
+
+	ret = i2c_smbus_write_byte_data(data->client, reg, val);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_fifo_config0\n");
+		return ret;
+	}
+
+	data->watermark = val;
+
+	return 0;
+}
+
+
+static int bmc150_accel_fifo_flush(struct iio_dev *indio_dev)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int ret, i;
+	u8 count;
+	u16 buffer[BMC150_ACCEL_FIFO_LENGTH * 3];
+	u8 reg_fifo_data = BMC150_ACCEL_REG_FIFO_DATA;
+	struct i2c_msg msg[2];
+	int64_t tstamp;
+	int sample_freq = 0, sec, ms;
+
+	ret = bmc150_accel_get_bw(data, &sec, &ms);
+	if (ret == IIO_VAL_INT_PLUS_MICRO)
+		sample_freq = sec * 1000000000 + ms * 1000;
+
+	ret = i2c_smbus_read_byte_data(data->client,
+				       BMC150_ACCEL_REG_FIFO_STATUS);
+	if (ret < 0) {
+		dev_err(&indio_dev->dev, "Error reading reg_fifo_status\n");
+		return ret;
+	}
+
+	count = ret & 0x7F;
+
+	if (!count)
+		return 0;
+
+	msg[0].addr = data->client->addr;
+	msg[0].flags = 0;
+	msg[0].buf = &reg_fifo_data;
+	msg[0].len = sizeof(reg_fifo_data);
+
+	msg[1].addr = data->client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = (u8 *)buffer;
+	msg[1].len = count * 3 * 2;
+
+	ret = i2c_transfer(data->client->adapter, msg, 2);
+	if (ret != 2) {
+		dev_err(&indio_dev->dev, "Error reading reg_fifo_data\n");
+		return ret;
+	}
+
+	if (!data->timestamp)
+		data->timestamp = iio_get_time_ns();
+
+	tstamp = data->timestamp - count * sample_freq;
+
+	for (i = 0; i < count; i++) {
+		u16 sample[8];
+		int j, bit;
+
+		j = 0;
+		for_each_set_bit(bit, indio_dev->buffer->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_freq;
+	}
+
+	data->timestamp = 0;
+
+	return 0;
+}
+
+static int bmc150_accel_fifo_mode_set(struct bmc150_accel_data *data)
+{
+	u8 reg = BMC150_ACCEL_REG_FIFO_CONFIG1;
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, reg);
+
+	/* writting the fifo config discards FIFO data - avoid it if possible */
+	if (ret == data->fifo_mode)
+		return 0;
+
+	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;
+
+	/* we can set the the watermark value only after FIFO is enabled */
+	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_fifo_setup(struct bmc150_accel_trigger *t, bool state)
+{
+	if (state)
+		t->data->fifo_mode = 0x40;
+	else
+		t->data->fifo_mode = 0;
+
+	return bmc150_accel_fifo_mode_set(t->data);
+}
+
+const struct iio_hwfifo bmc150_accel_hwfifo = {
+	.length = BMC150_ACCEL_FIFO_LENGTH,
+	.set_watermark = bmc150_accel_set_watermark,
+	.flush = bmc150_accel_fifo_flush,
+};
+
 static int bmc150_accel_probe(struct i2c_client *client,
 			      const struct i2c_device_id *id)
 {
@@ -1387,6 +1567,8 @@ static int bmc150_accel_probe(struct i2c_client *client,
 				"Failed: iio triggered buffer setup\n");
 			goto err_trigger_unregister;
 		}
+
+		indio_dev->hwfifo = &bmc150_accel_hwfifo;
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -1458,6 +1640,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_mode_set(data);
 	mutex_unlock(&data->mutex);
 
 	return 0;
@@ -1487,6 +1670,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_mode_set(data);
+	if (ret < 0)
+		return ret;
 
 	sleep_val = bmc150_accel_get_startup_times(data);
 	if (sleep_val < 20)
-- 
1.9.1