From 098d18739ef24ce71f55595901e7da1086d39794 Mon Sep 17 00:00:00 2001 From: mario tesi Date: Thu, 25 Nov 2021 14:16:30 +0100 Subject: [PATCH] drivers:iio:stm:accel: Add support to ST MEMS LIS3DHH/IIS3DHHC List of supported features: - ODRs 1100 Hz - Sensor Event Timestamp support - Read single accel raw data values - FIFO: > Accel data stored in FIFO > HW Watermark configuration > Custom Flush command / event support Signed-off-by: mario tesi Change-Id: I4f9cf84c2e934a09ce88e9e8a156c27c2421049f Reviewed-on: https://sczcxd1104.scz.st.com/gerrit/c/linux/stm-ldd-iio/+/333 Tested-by: aosp-ger Reviewed-by: Denis CIOCCA --- drivers/iio/stm/accel/Kconfig | 13 + drivers/iio/stm/accel/Makefile | 2 + drivers/iio/stm/accel/st_lis3dhh.c | 430 ++++++++++++++++++++++ drivers/iio/stm/accel/st_lis3dhh.h | 62 ++++ drivers/iio/stm/accel/st_lis3dhh_buffer.c | 275 ++++++++++++++ stm_iio_configs/lis3dhh_defconfig | 1 + 6 files changed, 783 insertions(+) create mode 100644 drivers/iio/stm/accel/st_lis3dhh.c create mode 100644 drivers/iio/stm/accel/st_lis3dhh.h create mode 100644 drivers/iio/stm/accel/st_lis3dhh_buffer.c create mode 100644 stm_iio_configs/lis3dhh_defconfig diff --git a/drivers/iio/stm/accel/Kconfig b/drivers/iio/stm/accel/Kconfig index 8249a4a28566..4df0152f91dc 100644 --- a/drivers/iio/stm/accel/Kconfig +++ b/drivers/iio/stm/accel/Kconfig @@ -127,4 +127,17 @@ config ST_ISM303DAC_ACCEL_IIO_LIMIT_FIFO can be limited by software or hardware. Set 0 to disable the limit. + +config IIO_ST_LIS3DHH + tristate "STMicroelectronics LIS3DHH/IIS3DHHC Accelerometer driver" + depends on SPI_MASTER && SYSFS + select IIO_BUFFER + select IIO_KFIFO_BUF + help + Say yes here to build support for STMicroelectronics LIS3DHH and + IIS3DHHC accelerometers + + This driver can also be built as a module. If so, will be named + st_lis3dhh + endmenu diff --git a/drivers/iio/stm/accel/Makefile b/drivers/iio/stm/accel/Makefile index 242e62ab87d5..f0ac2ecc0652 100644 --- a/drivers/iio/stm/accel/Makefile +++ b/drivers/iio/stm/accel/Makefile @@ -26,3 +26,5 @@ obj-$(CONFIG_IIO_ST_ISM303DAC_ACCEL) += ism303dac_accel.o obj-$(CONFIG_IIO_ST_ISM303DAC_ACCEL_I2C) += st_ism303dac_accel_i2c.o obj-$(CONFIG_IIO_ST_ISM303DAC_ACCEL_SPI) += st_ism303dac_accel_spi.o +st-lis3dhh-y := st_lis3dhh.o st_lis3dhh_buffer.o +obj-$(CONFIG_IIO_ST_LIS3DHH) += st-lis3dhh.o diff --git a/drivers/iio/stm/accel/st_lis3dhh.c b/drivers/iio/stm/accel/st_lis3dhh.c new file mode 100644 index 000000000000..7cb84abc1275 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis3dhh.c @@ -0,0 +1,430 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis3dhh sensor driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "st_lis3dhh.h" + +#define LIS3DHH_DEV_NAME "lis3dhh" +#define IIS3DHHC_DEV_NAME "iis3dhhc" + +#define REG_WHOAMI_ADDR 0x0f +#define REG_WHOAMI_VAL 0x11 + +#define REG_CTRL1_ADDR 0x20 +#define REG_CTRL1_BDU_MASK BIT(0) +#define REG_CTRL1_SW_RESET_MASK BIT(2) +#define REG_CTRL1_EN_MASK BIT(7) + +#define REG_INT1_CTRL_ADDR 0x21 +#define REG_INT2_CTRL_ADDR 0x22 +#define REG_INT_FTM_MASK BIT(3) + +#define ST_LIS3DHH_FS IIO_G_TO_M_S_2(76) + +#define ST_LIS3DHH_DATA_CHANNEL(addr, modx, scan_idx) \ +{ \ + .type = IIO_ACCEL, \ + .address = addr, \ + .modified = 1, \ + .channel2 = modx, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = scan_idx, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ +} + +#define ST_LIS3DHH_FLUSH_CHANNEL() \ +{ \ + .type = IIO_ACCEL, \ + .modified = 0, \ + .scan_index = -1, \ + .indexed = -1, \ + .event_spec = &st_lis3dhh_fifo_flush_event, \ + .num_event_specs = 1, \ +} + +const struct iio_event_spec st_lis3dhh_fifo_flush_event = { + .type = IIO_EV_TYPE_FIFO_FLUSH, + .dir = IIO_EV_DIR_EITHER, +}; + +static const struct iio_chan_spec st_lis3dhh_channels[] = { + ST_LIS3DHH_DATA_CHANNEL(0x28, IIO_MOD_X, 0), + ST_LIS3DHH_DATA_CHANNEL(0x2a, IIO_MOD_Y, 1), + ST_LIS3DHH_DATA_CHANNEL(0x2c, IIO_MOD_Z, 2), + ST_LIS3DHH_FLUSH_CHANNEL(), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +#define SENSORS_SPI_READ BIT(7) + int st_lis3dhh_spi_read(struct st_lis3dhh_hw *hw, u8 addr, int len, u8 *data) +{ + struct spi_device *spi = to_spi_device(hw->dev); + int err; + + struct spi_transfer xfers[] = { + { + .tx_buf = hw->tb.tx_buf, + .bits_per_word = 8, + .len = 1, + }, + { + .rx_buf = hw->tb.rx_buf, + .bits_per_word = 8, + .len = len, + } + }; + + hw->tb.tx_buf[0] = addr | SENSORS_SPI_READ; + err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers)); + if (err < 0) + return err; + + memcpy(data, hw->tb.rx_buf, len * sizeof(u8)); + + return len; +} + +static int st_lis3dhh_spi_write(struct st_lis3dhh_hw *hw, u8 addr, + int len, u8 *data) +{ + struct spi_device *spi = to_spi_device(hw->dev); + + if (len >= ST_LIS3DHH_TX_MAX_LENGTH) + return -ENOMEM; + + hw->tb.tx_buf[0] = addr; + memcpy(&hw->tb.tx_buf[1], data, len); + + return spi_write(spi, hw->tb.tx_buf, len + 1); +} + +int st_lis3dhh_write_with_mask(struct st_lis3dhh_hw *hw, u8 addr, u8 mask, + u8 val) +{ + u8 data; + int err; + + mutex_lock(&hw->lock); + + err = st_lis3dhh_spi_read(hw, addr, 1, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read %02x register\n", addr); + mutex_unlock(&hw->lock); + + return err; + } + + data = (data & ~mask) | ((val << __ffs(mask)) & mask); + + err = st_lis3dhh_spi_write(hw, addr, 1, &data); + if (err < 0) { + dev_err(hw->dev, "failed to write %02x register\n", addr); + mutex_unlock(&hw->lock); + + return err; + } + + mutex_unlock(&hw->lock); + + return 0; +} + +int st_lis3dhh_set_enable(struct st_lis3dhh_hw *hw, bool enable) +{ + return st_lis3dhh_write_with_mask(hw, REG_CTRL1_ADDR, + REG_CTRL1_EN_MASK, enable); +} + +static int st_lis3dhh_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lis3dhh_hw *hw = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: { + int err, delay; + u8 data[2]; + + mutex_lock(&iio_dev->mlock); + + if (iio_buffer_enabled(iio_dev)) { + mutex_unlock(&iio_dev->mlock); + return -EBUSY; + } + + err = st_lis3dhh_set_enable(hw, true); + if (err < 0) { + mutex_unlock(&iio_dev->mlock); + return err; + } + + /* sample to discard, 3 * odr us */ + delay = 3000000 / ST_LIS3DHH_ODR; + usleep_range(delay, delay + 1); + + err = st_lis3dhh_spi_read(hw, ch->address, 2, data); + if (err < 0) { + mutex_unlock(&iio_dev->mlock); + return err; + } + + err = st_lis3dhh_set_enable(hw, false); + if (err < 0) { + mutex_unlock(&iio_dev->mlock); + return err; + } + + *val = (s16)get_unaligned_le16(data); + *val = *val >> ch->scan_type.shift; + + mutex_unlock(&iio_dev->mlock); + + ret = IIO_VAL_INT; + break; + } + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = ST_LIS3DHH_FS; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = ST_LIS3DHH_ODR; + ret = IIO_VAL_INT; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static ssize_t +st_lis3dhh_get_sampling_frequency_avail(struct device *device, + struct device_attribute *attr, + char *buf) +{ + return scnprintf(buf, PAGE_SIZE, "%d\n", ST_LIS3DHH_ODR); +} + +static ssize_t st_lis3dhh_get_scale_avail(struct device *device, + struct device_attribute *attr, + char *buf) +{ + return scnprintf(buf, PAGE_SIZE, "0.%06d\n", (int)ST_LIS3DHH_FS); +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lis3dhh_get_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_lis3dhh_get_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, + st_lis3dhh_get_hwfifo_watermark, + st_lis3dhh_set_hwfifo_watermark, 0); +static IIO_DEVICE_ATTR(hwfifo_watermark_max, 0444, + st_lis3dhh_get_max_hwfifo_watermark, NULL, 0); +static IIO_DEVICE_ATTR(hwfifo_flush, 0200, NULL, st_lis3dhh_flush_hwfifo, 0); + +static struct attribute *st_lis3dhh_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark.dev_attr.attr, + &iio_dev_attr_hwfifo_watermark_max.dev_attr.attr, + &iio_dev_attr_hwfifo_flush.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lis3dhh_attribute_group = { + .attrs = st_lis3dhh_attributes, +}; + +static const struct iio_info st_lis3dhh_info = { + .attrs = &st_lis3dhh_attribute_group, + .read_raw = st_lis3dhh_read_raw, +}; + +static int st_lis3dhh_check_whoami(struct st_lis3dhh_hw *hw) +{ + u8 data; + int err; + + err = st_lis3dhh_spi_read(hw, REG_WHOAMI_ADDR, sizeof(data), &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + return err; + } + + if (data != REG_WHOAMI_VAL) { + dev_err(hw->dev, "wrong whoami {%02x-%02x}\n", + data, REG_WHOAMI_VAL); + return -ENODEV; + } + + return 0; +} + +static int st_lis3dhh_of_get_drdy_pin(struct st_lis3dhh_hw *hw, int *drdy_pin) +{ + struct device_node *np = hw->dev->of_node; + + if (!np) + return -EINVAL; + + return of_property_read_u32(np, "st,drdy-int-pin", drdy_pin); +} + +static int st_lis3dhh_set_drdy_reg(struct st_lis3dhh_hw *hw) +{ + int drdy_pin; + u8 drdy_reg; + + if (st_lis3dhh_of_get_drdy_pin(hw, &drdy_pin) < 0) { + struct st_sensors_platform_data *pdata; + struct device *dev = hw->dev; + + pdata = (struct st_sensors_platform_data *)dev->platform_data; + drdy_pin = pdata ? pdata->drdy_int_pin : 1; + } + + switch (drdy_pin) { + case 1: + drdy_reg = REG_INT1_CTRL_ADDR; + break; + case 2: + drdy_reg = REG_INT2_CTRL_ADDR; + break; + default: + dev_err(hw->dev, "unsupported data ready pin\n"); + return -EINVAL; + } + + return st_lis3dhh_write_with_mask(hw, drdy_reg, REG_INT_FTM_MASK, 1); +} + +static int st_lis3dhh_init_device(struct st_lis3dhh_hw *hw) +{ + int err; + + err = st_lis3dhh_write_with_mask(hw, REG_CTRL1_ADDR, + REG_CTRL1_SW_RESET_MASK, 1); + if (err < 0) + return err; + + msleep(200); + + err = st_lis3dhh_write_with_mask(hw, REG_CTRL1_ADDR, + REG_CTRL1_BDU_MASK, 1); + if (err < 0) + return err; + + err = st_lis3dhh_update_watermark(hw, hw->watermark); + if (err < 0) + return err; + + return st_lis3dhh_set_drdy_reg(hw); +} + +static int st_lis3dhh_spi_probe(struct spi_device *spi) +{ + struct st_lis3dhh_hw *hw; + struct iio_dev *iio_dev; + int err; + + iio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*hw)); + if (!iio_dev) + return -ENOMEM; + + spi_set_drvdata(spi, iio_dev); + + iio_dev->channels = st_lis3dhh_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lis3dhh_channels); + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->info = &st_lis3dhh_info; + iio_dev->dev.parent = &spi->dev; + iio_dev->name = spi->modalias; + + hw = iio_priv(iio_dev); + + mutex_init(&hw->fifo_lock); + mutex_init(&hw->lock); + + hw->watermark = 1; + hw->dev = &spi->dev; + hw->name = spi->modalias; + hw->irq = spi->irq; + hw->iio_dev = iio_dev; + + err = st_lis3dhh_check_whoami(hw); + if (err < 0) + return err; + + err = st_lis3dhh_init_device(hw); + if (err < 0) + return err; + + if (hw->irq > 0) { + err = st_lis3dhh_fifo_setup(hw); + if (err < 0) + return err; + } + + return devm_iio_device_register(hw->dev, iio_dev); +} + +static const struct of_device_id st_lis3dhh_spi_of_match[] = { + { + .compatible = "st,lis3dhh", + .data = LIS3DHH_DEV_NAME, + }, + { + .compatible = "st,iis3dhhc", + .data = IIS3DHHC_DEV_NAME, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lis3dhh_spi_of_match); + +static const struct spi_device_id st_lis3dhh_spi_id_table[] = { + { LIS3DHH_DEV_NAME }, + { IIS3DHHC_DEV_NAME }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_lis3dhh_spi_id_table); + +static struct spi_driver st_lis3dhh_driver = { + .driver = { + .name = "st_lis3dhh", + .of_match_table = of_match_ptr(st_lis3dhh_spi_of_match), + }, + .probe = st_lis3dhh_spi_probe, + .id_table = st_lis3dhh_spi_id_table, +}; +module_spi_driver(st_lis3dhh_driver); + +MODULE_AUTHOR("Lorenzo Bianconi "); +MODULE_DESCRIPTION("STMicroelectronics st_lis3dhh sensor driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/stm/accel/st_lis3dhh.h b/drivers/iio/stm/accel/st_lis3dhh.h new file mode 100644 index 000000000000..c7553e2e761f --- /dev/null +++ b/drivers/iio/stm/accel/st_lis3dhh.h @@ -0,0 +1,62 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * STMicroelectronics st_lis3dhh sensor driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi + */ + +#ifndef ST_LIS3DHH_H +#define ST_LIS3DHH_H + + +#include + +#define ST_LIS3DHH_DATA_SIZE 6 +#define ST_LIS3DHH_RX_MAX_LENGTH 96 +#define ST_LIS3DHH_TX_MAX_LENGTH 8 + +#define ST_LIS3DHH_ODR 1100 + +struct st_lis3dhh_transfer_buffer { + u8 rx_buf[ST_LIS3DHH_RX_MAX_LENGTH]; + u8 tx_buf[ST_LIS3DHH_TX_MAX_LENGTH] ____cacheline_aligned; +}; + +struct st_lis3dhh_hw { + struct device *dev; + const char *name; + int irq; + + struct mutex fifo_lock; + struct mutex lock; + + u8 watermark; + s64 delta_ts; + s64 ts_irq; + s64 ts; + struct iio_dev *iio_dev; + struct st_lis3dhh_transfer_buffer tb; +}; + +int st_lis3dhh_write_with_mask(struct st_lis3dhh_hw *hw, u8 addr, u8 mask, + u8 val); + int st_lis3dhh_spi_read(struct st_lis3dhh_hw *hw, u8 addr, int len, u8 *data); +int st_lis3dhh_set_enable(struct st_lis3dhh_hw *hw, bool enable); +int st_lis3dhh_fifo_setup(struct st_lis3dhh_hw *hw); +ssize_t st_lis3dhh_flush_hwfifo(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size); +ssize_t st_lis3dhh_get_max_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, + char *buf); +ssize_t st_lis3dhh_get_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + char *buf); +ssize_t st_lis3dhh_set_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size); +int st_lis3dhh_update_watermark(struct st_lis3dhh_hw *hw, u8 watermark); + +#endif /* ST_LIS3DHH_H */ diff --git a/drivers/iio/stm/accel/st_lis3dhh_buffer.c b/drivers/iio/stm/accel/st_lis3dhh_buffer.c new file mode 100644 index 000000000000..6a02852d91b1 --- /dev/null +++ b/drivers/iio/stm/accel/st_lis3dhh_buffer.c @@ -0,0 +1,275 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * STMicroelectronics st_lis3dhh sensor driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "st_lis3dhh.h" + +#define REG_CTRL3_ADDR 0x23 +#define REG_CTRL5_ACC_FIFO_EN_MASK BIT(1) + +#define REG_FIFO_CTRL_REG 0x2e +#define REG_FIFO_CTRL_REG_WTM_MASK GENMASK(4, 0) +#define REG_FIFO_CTRL_MODE_MASK GENMASK(7, 5) + +#define REG_FIFO_SRC_ADDR 0x2f +#define REG_FIFO_SRC_OVR_MASK BIT(6) +#define REG_FIFO_SRC_FSS_MASK GENMASK(5, 0) + +#define ST_LIS3DHH_MAX_WATERMARK 28 + +enum st_lis3dhh_fifo_mode { + ST_LIS3DHH_FIFO_BYPASS = 0x0, + ST_LIS3DHH_FIFO_STREAM = 0x6, +}; + +static inline s64 st_lis3dhh_get_timestamp(struct st_lis3dhh_hw *hw) +{ + return iio_get_time_ns(hw->iio_dev); +} + +#define ST_LIS3DHH_EWMA_LEVEL 120 +#define ST_LIS3DHH_EWMA_DIV 128 +static inline s64 st_lis3dhh_ewma(s64 old, s64 new, int weight) +{ + s64 diff, incr; + + diff = new - old; + incr = div_s64((ST_LIS3DHH_EWMA_DIV - weight) * diff, + ST_LIS3DHH_EWMA_DIV); + + return old + incr; +} + +static int st_lis3dhh_set_fifo_mode(struct st_lis3dhh_hw *hw, + enum st_lis3dhh_fifo_mode mode) +{ + return st_lis3dhh_write_with_mask(hw, REG_FIFO_CTRL_REG, + REG_FIFO_CTRL_MODE_MASK, mode); +} + +static int st_lis3dhh_read_fifo(struct st_lis3dhh_hw *hw) +{ + u8 iio_buff[ALIGN(ST_LIS3DHH_DATA_SIZE, sizeof(s64)) + sizeof(s64)]; + u8 buff[ST_LIS3DHH_RX_MAX_LENGTH], data, nsamples; + struct iio_dev *iio_dev = hw->iio_dev; + struct iio_chan_spec const *ch = iio_dev->channels; + int i, err, word_len, fifo_len, read_len = 0; + + err = st_lis3dhh_spi_read(hw, REG_FIFO_SRC_ADDR, sizeof(data), &data); + if (err < 0) + return err; + + nsamples = data & REG_FIFO_SRC_FSS_MASK; + fifo_len = nsamples * ST_LIS3DHH_DATA_SIZE; + + while (read_len < fifo_len) { + word_len = min_t(int, fifo_len - read_len, sizeof(buff)); + err = st_lis3dhh_spi_read(hw, ch[0].address, word_len, buff); + if (err < 0) + return err; + + for (i = 0; i < word_len; i += ST_LIS3DHH_DATA_SIZE) { + memcpy(iio_buff, &buff[i], ST_LIS3DHH_DATA_SIZE); + iio_push_to_buffers_with_timestamp(iio_dev, iio_buff, + hw->ts); + hw->ts += hw->delta_ts; + } + read_len += word_len; + } + + return read_len; +} + +ssize_t st_lis3dhh_flush_hwfifo(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(device); + struct st_lis3dhh_hw *hw = iio_priv(iio_dev); + s64 code; + int err; + + mutex_lock(&hw->fifo_lock); + + err = st_lis3dhh_read_fifo(hw); + hw->ts_irq = st_lis3dhh_get_timestamp(hw); + + mutex_unlock(&hw->fifo_lock); + + code = IIO_UNMOD_EVENT_CODE(IIO_ACCEL, -1, + IIO_EV_TYPE_FIFO_FLUSH, + IIO_EV_DIR_EITHER); + iio_push_event(iio_dev, code, hw->ts_irq); + + return err < 0 ? err : size; +} + +ssize_t st_lis3dhh_get_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *iio_dev = dev_get_drvdata(device); + struct st_lis3dhh_hw *hw = iio_priv(iio_dev); + + return sprintf(buf, "%d\n", hw->watermark); +} + +int st_lis3dhh_update_watermark(struct st_lis3dhh_hw *hw, u8 watermark) +{ + return st_lis3dhh_write_with_mask(hw, REG_FIFO_CTRL_REG, + REG_FIFO_CTRL_REG_WTM_MASK, + watermark); +} + +ssize_t st_lis3dhh_set_hwfifo_watermark(struct device *device, + struct device_attribute *attr, + const char *buf, size_t size) +{ + struct iio_dev *iio_dev = dev_get_drvdata(device); + struct st_lis3dhh_hw *hw = iio_priv(iio_dev); + int err, val; + + mutex_lock(&iio_dev->mlock); + if (iio_buffer_enabled(iio_dev)) { + err = -EBUSY; + goto unlock; + } + + err = kstrtoint(buf, 10, &val); + if (err < 0) + goto unlock; + + if (val < 1 || val > ST_LIS3DHH_MAX_WATERMARK) { + err = -EINVAL; + goto unlock; + } + + err = st_lis3dhh_update_watermark(hw, val); + if (err < 0) + goto unlock; + + hw->watermark = val; + +unlock: + mutex_unlock(&iio_dev->mlock); + + return err < 0 ? err : size; +} + +ssize_t st_lis3dhh_get_max_hwfifo_watermark(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + return sprintf(buf, "%d\n", ST_LIS3DHH_MAX_WATERMARK); +} + +static irqreturn_t st_lis3dhh_buffer_handler_irq(int irq, void *private) +{ + struct st_lis3dhh_hw *hw = private; + s64 ts, delta_ts; + + ts = st_lis3dhh_get_timestamp(hw); + delta_ts = div_s64(ts - hw->ts_irq, hw->watermark); + hw->delta_ts = st_lis3dhh_ewma(hw->delta_ts, delta_ts, + ST_LIS3DHH_EWMA_LEVEL); + hw->ts_irq = ts; + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t st_lis3dhh_buffer_handler_thread(int irq, void *private) +{ + struct st_lis3dhh_hw *hw = private; + + mutex_lock(&hw->fifo_lock); + st_lis3dhh_read_fifo(hw); + mutex_unlock(&hw->fifo_lock); + + return IRQ_HANDLED; +} + +static int st_lis3dhh_update_fifo(struct st_lis3dhh_hw *hw, bool enable) +{ + enum st_lis3dhh_fifo_mode mode; + int err; + + if (enable) { + hw->ts_irq = hw->ts = st_lis3dhh_get_timestamp(hw); + hw->delta_ts = div_s64(1000000000LL, ST_LIS3DHH_ODR); + } + + err = st_lis3dhh_write_with_mask(hw, REG_CTRL3_ADDR, + REG_CTRL5_ACC_FIFO_EN_MASK, enable); + if (err < 0) + return err; + + mode = enable ? ST_LIS3DHH_FIFO_STREAM : ST_LIS3DHH_FIFO_BYPASS; + err = st_lis3dhh_set_fifo_mode(hw, mode); + if (err < 0) + return err; + + return st_lis3dhh_set_enable(hw, enable); +} + +static int st_lis3dhh_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_lis3dhh_update_fifo(iio_priv(iio_dev), true); +} + +static int st_lis3dhh_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_lis3dhh_update_fifo(iio_priv(iio_dev), false); +} + +static const struct iio_buffer_setup_ops st_lis3dhh_buffer_ops = { + .preenable = st_lis3dhh_buffer_preenable, + .postdisable = st_lis3dhh_buffer_postdisable, +}; + +int st_lis3dhh_fifo_setup(struct st_lis3dhh_hw *hw) +{ + struct iio_dev *iio_dev = hw->iio_dev; + struct iio_buffer *buffer; + int ret; + + ret = devm_request_threaded_irq(hw->dev, hw->irq, + st_lis3dhh_buffer_handler_irq, + st_lis3dhh_buffer_handler_thread, + IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + hw->name, hw); + if (ret) { + dev_err(hw->dev, "failed to request trigger irq %d\n", + hw->irq); + return ret; + } + + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(iio_dev, buffer); + iio_dev->setup_ops = &st_lis3dhh_buffer_ops; + iio_dev->modes |= INDIO_BUFFER_SOFTWARE; + + return 0; +} + +MODULE_AUTHOR("Lorenzo Bianconi "); +MODULE_DESCRIPTION("STMicroelectronics st_lis3dhh buffer driver"); +MODULE_LICENSE("GPL v2"); diff --git a/stm_iio_configs/lis3dhh_defconfig b/stm_iio_configs/lis3dhh_defconfig new file mode 100644 index 000000000000..d75e554a1832 --- /dev/null +++ b/stm_iio_configs/lis3dhh_defconfig @@ -0,0 +1 @@ +CONFIG_IIO_ST_LIS3DHH=m