android/android_kernel_samsung_sm8650
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

drivers:iio:stm:accel: Add support to ST MEMS LIS3DHH/IIS3DHHC

Browse Source 
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 <mario.tesi@st.com>
Change-Id: I4f9cf84c2e934a09ce88e9e8a156c27c2421049f
Reviewed-on: https://sczcxd1104.scz.st.com/gerrit/c/linux/stm-ldd-iio/+/333
Tested-by: aosp-ger <aosp-ger@st.com>
Reviewed-by: Denis CIOCCA <denis.ciocca@st.com>
This commit is contained in:
mario tesi 2021-11-25 14:16:30 +01:00 committed by Mario Tesi
parent 863aa3c1a2
commit 098d18739e
6 changed files with 783 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
drivers/iio/stm/accel/Kconfig
View File

@ -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

2
drivers/iio/stm/accel/Makefile
View File

@ -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

430
drivers/iio/stm/accel/st_lis3dhh.c Normal file
View File

@ -0,0 +1,430 @@
// SPDX-License-Identifier: GPL-2.0-only
/*
* STMicroelectronics st_lis3dhh sensor driver
*
* Copyright 2016 STMicroelectronics Inc.
*
* Lorenzo Bianconi <lorenzo.bianconi@st.com>
*/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/device.h>
#include <linux/iio/sysfs.h>
#include <linux/iio/buffer.h>
#include <linux/delay.h>
#include <linux/interrupt.h>
#include <linux/spi/spi.h>
#include <asm/unaligned.h>
#include <linux/platform_data/st_sensors_pdata.h>
#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 <lorenzo.bianconi@st.com>");
MODULE_DESCRIPTION("STMicroelectronics st_lis3dhh sensor driver");
MODULE_LICENSE("GPL v2");

62
drivers/iio/stm/accel/st_lis3dhh.h Normal file
View File

@ -0,0 +1,62 @@
/* SPDX-License-Identifier: GPL-2.0-only */
/*
* STMicroelectronics st_lis3dhh sensor driver
*
* Copyright 2016 STMicroelectronics Inc.
*
* Lorenzo Bianconi <lorenzo.bianconi@st.com>
*/
#ifndef ST_LIS3DHH_H
#define ST_LIS3DHH_H
#include <linux/iio/iio.h>
#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 */

275
drivers/iio/stm/accel/st_lis3dhh_buffer.c Normal file
View File

@ -0,0 +1,275 @@
// SPDX-License-Identifier: GPL-2.0-only
/*
* STMicroelectronics st_lis3dhh sensor driver
*
* Copyright 2016 STMicroelectronics Inc.
*
* Lorenzo Bianconi <lorenzo.bianconi@st.com>
*/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/device.h>
#include <linux/interrupt.h>
#include <linux/iio/events.h>
#include <linux/iio/iio.h>
#include <linux/iio/kfifo_buf.h>
#include <linux/iio/buffer.h>
#include <linux/iio/trigger.h>
#include <linux/iio/trigger_consumer.h>
#include <linux/iio/triggered_buffer.h>
#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 <lorenzo.bianconi@st.com>");
MODULE_DESCRIPTION("STMicroelectronics st_lis3dhh buffer driver");
MODULE_LICENSE("GPL v2");

1
stm_iio_configs/lis3dhh_defconfig Normal file
View File

@ -0,0 +1 @@
CONFIG_IIO_ST_LIS3DHH=m