diff --git a/drivers/iio/accel/fxls8962af-core.c b/drivers/iio/accel/fxls8962af-core.c index b909ba23e47c..3ea8488ed4c2 100644 --- a/drivers/iio/accel/fxls8962af-core.c +++ b/drivers/iio/accel/fxls8962af-core.c @@ -20,13 +20,16 @@ #include #include +#include #include +#include #include #include "fxls8962af.h" #define FXLS8962AF_INT_STATUS 0x00 #define FXLS8962AF_INT_STATUS_SRC_BOOT BIT(0) +#define FXLS8962AF_INT_STATUS_SRC_BUF BIT(5) #define FXLS8962AF_INT_STATUS_SRC_DRDY BIT(7) #define FXLS8962AF_TEMP_OUT 0x01 #define FXLS8962AF_VECM_LSB 0x02 @@ -34,6 +37,9 @@ #define FXLS8962AF_OUT_Y_LSB 0x06 #define FXLS8962AF_OUT_Z_LSB 0x08 #define FXLS8962AF_BUF_STATUS 0x0b +#define FXLS8962AF_BUF_STATUS_BUF_CNT GENMASK(5, 0) +#define FXLS8962AF_BUF_STATUS_BUF_OVF BIT(6) +#define FXLS8962AF_BUF_STATUS_BUF_WMRK BIT(7) #define FXLS8962AF_BUF_X_LSB 0x0c #define FXLS8962AF_BUF_Y_LSB 0x0e #define FXLS8962AF_BUF_Z_LSB 0x10 @@ -66,6 +72,7 @@ #define FXLS8962AF_ASLP_COUNT_LSB 0x1e #define FXLS8962AF_INT_EN 0x20 +#define FXLS8962AF_INT_EN_BUF_EN BIT(6) #define FXLS8962AF_INT_PIN_SEL 0x21 #define FXLS8962AF_INT_PIN_SEL_MASK GENMASK(7, 0) #define FXLS8962AF_INT_PIN_SEL_INT1 0x00 @@ -76,7 +83,10 @@ #define FXLS8962AF_OFF_Z 0x24 #define FXLS8962AF_BUF_CONFIG1 0x26 +#define FXLS8962AF_BC1_BUF_MODE_MASK GENMASK(6, 5) +#define FXLS8962AF_BC1_BUF_MODE_PREP(x) FIELD_PREP(FXLS8962AF_BC1_BUF_MODE_MASK, (x)) #define FXLS8962AF_BUF_CONFIG2 0x27 +#define FXLS8962AF_BUF_CONFIG2_BUF_WMRK GENMASK(5, 0) #define FXLS8962AF_ORIENT_STATUS 0x28 #define FXLS8962AF_ORIENT_CONFIG 0x29 @@ -106,6 +116,7 @@ #define FXLS8962AF_AUTO_SUSPEND_DELAY_MS 2000 +#define FXLS8962AF_FIFO_LENGTH 32 #define FXLS8962AF_SCALE_TABLE_LEN 4 #define FXLS8962AF_SAMP_FREQ_TABLE_LEN 13 @@ -133,7 +144,13 @@ struct fxls8962af_data { struct regmap *regmap; const struct fxls8962af_chip_info *chip_info; struct regulator *vdd_reg; + struct { + __le16 channels[3]; + s64 ts __aligned(8); + } scan; + int64_t timestamp, old_timestamp; /* Only used in hw fifo mode. */ struct iio_mount_matrix orientation; + u8 watermark; }; const struct regmap_config fxls8962af_regmap_conf = { @@ -433,6 +450,18 @@ static int fxls8962af_write_raw(struct iio_dev *indio_dev, } } +static int fxls8962af_set_watermark(struct iio_dev *indio_dev, unsigned val) +{ + struct fxls8962af_data *data = iio_priv(indio_dev); + + if (val > FXLS8962AF_FIFO_LENGTH) + val = FXLS8962AF_FIFO_LENGTH; + + data->watermark = val; + + return 0; +} + #define FXLS8962AF_CHANNEL(axis, reg, idx) { \ .type = IIO_ACCEL, \ .address = reg, \ @@ -493,6 +522,7 @@ static const struct iio_info fxls8962af_info = { .write_raw = &fxls8962af_write_raw, .write_raw_get_fmt = fxls8962af_write_raw_get_fmt, .read_avail = fxls8962af_read_avail, + .hwfifo_set_watermark = fxls8962af_set_watermark, }; static int fxls8962af_reset(struct fxls8962af_data *data) @@ -517,6 +547,157 @@ static int fxls8962af_reset(struct fxls8962af_data *data) return ret; } +static int __fxls8962af_fifo_set_mode(struct fxls8962af_data *data, bool onoff) +{ + int ret; + + /* Enable watermark at max fifo size */ + ret = regmap_update_bits(data->regmap, FXLS8962AF_BUF_CONFIG2, + FXLS8962AF_BUF_CONFIG2_BUF_WMRK, + data->watermark); + if (ret) + return ret; + + return regmap_update_bits(data->regmap, FXLS8962AF_BUF_CONFIG1, + FXLS8962AF_BC1_BUF_MODE_MASK, + FXLS8962AF_BC1_BUF_MODE_PREP(onoff)); +} + +static int fxls8962af_buffer_preenable(struct iio_dev *indio_dev) +{ + return fxls8962af_power_on(iio_priv(indio_dev)); +} + +static int fxls8962af_buffer_postenable(struct iio_dev *indio_dev) +{ + struct fxls8962af_data *data = iio_priv(indio_dev); + int ret; + + fxls8962af_standby(data); + + /* Enable buffer interrupt */ + ret = regmap_update_bits(data->regmap, FXLS8962AF_INT_EN, + FXLS8962AF_INT_EN_BUF_EN, + FXLS8962AF_INT_EN_BUF_EN); + if (ret) + return ret; + + ret = __fxls8962af_fifo_set_mode(data, true); + + fxls8962af_active(data); + + return ret; +} + +static int fxls8962af_buffer_predisable(struct iio_dev *indio_dev) +{ + struct fxls8962af_data *data = iio_priv(indio_dev); + int ret; + + fxls8962af_standby(data); + + /* Disable buffer interrupt */ + ret = regmap_update_bits(data->regmap, FXLS8962AF_INT_EN, + FXLS8962AF_INT_EN_BUF_EN, 0); + if (ret) + return ret; + + ret = __fxls8962af_fifo_set_mode(data, false); + + fxls8962af_active(data); + + return ret; +} + +static int fxls8962af_buffer_postdisable(struct iio_dev *indio_dev) +{ + struct fxls8962af_data *data = iio_priv(indio_dev); + + return fxls8962af_power_off(data); +} + +static const struct iio_buffer_setup_ops fxls8962af_buffer_ops = { + .preenable = fxls8962af_buffer_preenable, + .postenable = fxls8962af_buffer_postenable, + .predisable = fxls8962af_buffer_predisable, + .postdisable = fxls8962af_buffer_postdisable, +}; + +static int fxls8962af_fifo_transfer(struct fxls8962af_data *data, + u16 *buffer, int samples) +{ + struct device *dev = regmap_get_device(data->regmap); + int sample_length = 3 * sizeof(*buffer); + int ret; + int total_length = samples * sample_length; + + ret = regmap_raw_read(data->regmap, FXLS8962AF_BUF_X_LSB, buffer, + total_length); + if (ret) + dev_err(dev, "Error transferring data from fifo: %d\n", ret); + + return ret; +} + +static int fxls8962af_fifo_flush(struct iio_dev *indio_dev) +{ + struct fxls8962af_data *data = iio_priv(indio_dev); + struct device *dev = regmap_get_device(data->regmap); + u16 buffer[FXLS8962AF_FIFO_LENGTH * 3]; + uint64_t sample_period; + unsigned int reg; + int64_t tstamp; + int ret, i; + u8 count; + + ret = regmap_read(data->regmap, FXLS8962AF_BUF_STATUS, ®); + if (ret) + return ret; + + if (reg & FXLS8962AF_BUF_STATUS_BUF_OVF) { + dev_err(dev, "Buffer overflow"); + return -EOVERFLOW; + } + + count = reg & FXLS8962AF_BUF_STATUS_BUF_CNT; + if (!count) + return 0; + + data->old_timestamp = data->timestamp; + data->timestamp = iio_get_time_ns(indio_dev); + + /* + * Approximate timestamps for each of the sample based on the sampling, + * frequency, timestamp for last sample and number of samples. + */ + sample_period = (data->timestamp - data->old_timestamp); + do_div(sample_period, count); + tstamp = data->timestamp - (count - 1) * sample_period; + + ret = fxls8962af_fifo_transfer(data, buffer, count); + if (ret) + return ret; + + /* Demux hw FIFO into kfifo. */ + for (i = 0; i < count; i++) { + int j, bit; + + j = 0; + for_each_set_bit(bit, indio_dev->active_scan_mask, + indio_dev->masklength) { + memcpy(&data->scan.channels[j++], &buffer[i * 3 + bit], + sizeof(data->scan.channels[0])); + } + + iio_push_to_buffers_with_timestamp(indio_dev, &data->scan, + tstamp); + + tstamp += sample_period; + } + + return count; +} + static irqreturn_t fxls8962af_interrupt(int irq, void *p) { struct iio_dev *indio_dev = p; @@ -528,6 +709,14 @@ static irqreturn_t fxls8962af_interrupt(int irq, void *p) if (ret) return IRQ_NONE; + if (reg & FXLS8962AF_INT_STATUS_SRC_BUF) { + ret = fxls8962af_fifo_flush(indio_dev); + if (ret) + return IRQ_NONE; + + return IRQ_HANDLED; + } + return IRQ_NONE; } @@ -694,6 +883,12 @@ int fxls8962af_core_probe(struct device *dev, struct regmap *regmap, int irq) ret = fxls8962af_irq_setup(indio_dev, irq); if (ret) return ret; + + ret = devm_iio_kfifo_buffer_setup(dev, indio_dev, + INDIO_BUFFER_SOFTWARE, + &fxls8962af_buffer_ops); + if (ret) + return ret; } ret = pm_runtime_set_active(dev);