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

drivers:iio:imu: added support to LSM6DSM/L IMU sensor

Browse Source 
List of supported features:
 - Acc / Gyro sensors
    > Configurable ODRs [Power_Off ... 833] Hz
    > Configurable Full Scales
    > Sensor Event Timestamp
    > Read single raw data values
 - FIFO:
    > HW Watermark configuration
    > Custom Flush command / event support
 - Support to IIO trigger
 - HW SHUB enabled:
    > Samples in HW FIFO
    > Device supported:
      LIS3MDL, AKM09911, AKM09912, AKM09916, LPS22HB, LIS2MDL
 - Embedded event sensors:
    > Tilt sensor
    > Wrist Tilt sensor
    > Step Counter
    > Step Detection
    > Significant Motion
    > Tap
    > Double Tap

Signed-off-by: mario tesi <mario.tesi@st.com>
Change-Id: Ia531cc898a97735808c276ddb434e45fd5b83251
This commit is contained in:
mario tesi 2021-11-09 16:06:26 +01:00 committed by Mario Tesi
parent 5ba1426892
commit 2f7a9176d5
12 changed files with 6988 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
drivers/iio/stm/imu/Kconfig
View File

@ -14,5 +14,6 @@ source "drivers/iio/stm/imu/st_lsm6dso32x/Kconfig"
source "drivers/iio/stm/imu/st_lsm6dso/Kconfig"
source "drivers/iio/stm/imu/st_imu68/Kconfig"
source "drivers/iio/stm/imu/st_ism330dlc/Kconfig"
source "drivers/iio/stm/imu/st_lsm6dsm/Kconfig"
endmenu

1
drivers/iio/stm/imu/Makefile
View File

@ -12,3 +12,4 @@ obj-y += st_lsm6dso32x/
obj-y += st_lsm6dso/
obj-y += st_imu68/
obj-y += st_ism330dlc/
obj-y += st_lsm6dsm/

96
drivers/iio/stm/imu/st_lsm6dsm/Kconfig Normal file
View File

@ -0,0 +1,96 @@
# SPDX-License-Identifier: GPL-2.0-only
#
# st-lsm6dsm drivers for STMicroelectronics combo sensor
#
menuconfig ST_LSM6DSM_IIO
tristate "STMicroelectronics LSM6DSM/LSM6DSL sensor"
depends on (I2C || SPI) && SYSFS
select IIO_BUFFER
select IIO_TRIGGERED_BUFFER
select ST_LSM6DSM_I2C_IIO if (I2C)
select ST_LSM6DSM_SPI_IIO if (SPI)
help
This driver supports LSM6DSM and LSM6DSL sensors.
It is a gyroscope/accelerometer combo device.
This driver can be built as a module. The module will be called
st-lsm6dsm.
if ST_LSM6DSM_IIO
config ST_LSM6DSM_I2C_IIO
tristate
depends on ST_LSM6DSM_IIO
depends on I2C
config ST_LSM6DSM_SPI_IIO
tristate
depends on ST_LSM6DSM_IIO
depends on SPI
config ST_LSM6DSM_IIO_LIMIT_FIFO
int "Limit fifo read lenght (#n byte)"
depends on ST_LSM6DSM_IIO
range 0 4096
default 0
help
Limit atomic fifo read to #n byte. In some platform i2c/spi read
can be limited by software or hardware.
Set 0 to disable the limit.
config ST_LSM6DSM_STEP_COUNTER_ON_DURING_SUSPEND
bool "Keep Step counter on during suspend"
depends on ST_LSM6DSM_IIO
default n
help
During suspend step counter is kept on if enabled. Only interrupt
is disabled.
menuconfig ST_LSM6DSM_IIO_MASTER_SUPPORT
bool "I2C master controller"
depends on I2C && ST_LSM6DSM_IIO
default n
help
Added support for I2C master controller. Only one slave sensor is
supported.
if ST_LSM6DSM_IIO_MASTER_SUPPORT
config ST_LSM6DSM_ENABLE_INTERNAL_PULLUP
bool "Enabled internals pull-up resistors"
default y
choice
prompt "External sensor 0"
default ST_LSM6DSM_IIO_EXT0_LIS3MDL
help
Choose the external sensor 0 connected to LSM6DS3.
config ST_LSM6DSM_IIO_EXT0_LIS3MDL
bool "LIS3MDL"
config ST_LSM6DSM_IIO_EXT0_AKM09911
bool "AKM09911"
config ST_LSM6DSM_IIO_EXT0_AKM09912
bool "AKM09912"
config ST_LSM6DSM_IIO_EXT0_AKM09916
bool "AKM09916"
config ST_LSM6DSM_IIO_EXT0_LPS22HB
bool "LPS22HB"
config ST_LSM6DSM_IIO_EXT0_LIS2MDL
bool "LIS2MDL"
endchoice
endif
config ST_LSM6DSM_XL_DATA_INJECTION
bool "Enable XL data injection support"
depends on ST_LSM6DSM_IIO
default n
help
This option enables the accelerometer data injection
support. The device functions may so use an injected
pattern instead of taking the real sensor data.
endif #ST_LSM6DSM_IIO

13
drivers/iio/stm/imu/st_lsm6dsm/Makefile Normal file
View File

@ -0,0 +1,13 @@
# SPDX-License-Identifier: GPL-2.0-only
#
# Makefile for STMicroelectronics LSM6DSM sensor.
#
obj-$(CONFIG_ST_LSM6DSM_IIO) += st_lsm6dsm.o
st_lsm6dsm-objs := st_lsm6dsm_core.o
obj-$(CONFIG_ST_LSM6DSM_I2C_IIO) += st_lsm6dsm_i2c.o
obj-$(CONFIG_ST_LSM6DSM_SPI_IIO) += st_lsm6dsm_spi.o
st_lsm6dsm-$(CONFIG_IIO_BUFFER) += st_lsm6dsm_buffer.o
st_lsm6dsm-$(CONFIG_IIO_TRIGGER) += st_lsm6dsm_trigger.o
st_lsm6dsm-$(CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT) += st_lsm6dsm_i2c_master.o

366
drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm.h Normal file
View File

@ -0,0 +1,366 @@
/* SPDX-License-Identifier: GPL-2.0-only */
/*
* STMicroelectronics lsm6dsm driver
*
* Copyright 2016 STMicroelectronics Inc.
*
* Denis Ciocca <denis.ciocca@st.com>
*/
#ifndef ST_LSM6DSM_H
#define ST_LSM6DSM_H
#include <linux/types.h>
#include <linux/iio/trigger.h>
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
#include <linux/i2c.h>
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
#define LSM6DSM_DEV_NAME "lsm6dsm"
#define LSM6DSL_DEV_NAME "lsm6dsl"
enum st_mask_id {
ST_MASK_ID_ACCEL = 0,
ST_MASK_ID_GYRO,
ST_MASK_ID_SIGN_MOTION,
ST_MASK_ID_STEP_COUNTER,
ST_MASK_ID_STEP_DETECTOR,
ST_MASK_ID_TILT,
ST_MASK_ID_WTILT,
ST_MASK_ID_TAP,
ST_MASK_ID_TAP_TAP,
ST_MASK_ID_EXT0,
ST_MASK_ID_HW_PEDOMETER,
ST_MASK_ID_SENSOR_HUB,
ST_MASK_ID_DIGITAL_FUNC,
ST_MASK_ID_SENSOR_HUB_ASYNC_OP,
};
#define ST_INDIO_DEV_NUM 9
#define ST_LSM6DSM_TX_MAX_LENGTH 12
#define ST_LSM6DSM_RX_MAX_LENGTH 4097
#define ST_LSM6DSM_BYTE_FOR_CHANNEL 2
#define ST_LSM6DSM_BYTE_FOR_WRIST_TILT 1
#define ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE 6
#define ST_LSM6DSM_MAX_FIFO_SIZE 4096
#define ST_LSM6DSM_MAX_FIFO_THRESHOLD 546
#define ST_LSM6DSM_MAX_FIFO_LENGHT (ST_LSM6DSM_MAX_FIFO_SIZE / \
ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE)
#define ST_LSM6DSM_SELFTEST_NA_MS "na"
#define ST_LSM6DSM_SELFTEST_FAIL_MS "fail"
#define ST_LSM6DSM_SELFTEST_PASS_MS "pass"
#define ST_LSM6DSM_WAKE_UP_SENSORS (BIT(ST_MASK_ID_SIGN_MOTION) | \
BIT(ST_MASK_ID_TILT) | BIT(ST_MASK_ID_WTILT))
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
#define ST_LSM6DSM_NUM_CLIENTS 1
#else /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
#define ST_LSM6DSM_NUM_CLIENTS 0
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
#define ST_LSM6DSM_LSM_CHANNELS(device_type, modif, index, mod, \
endian, sbits, rbits, addr, s) \
{ \
.type = device_type, \
.modified = modif, \
.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
BIT(IIO_CHAN_INFO_SCALE), \
.scan_index = index, \
.channel2 = mod, \
.address = addr, \
.scan_type = { \
.sign = s, \
.realbits = rbits, \
.shift = sbits - rbits, \
.storagebits = sbits, \
.endianness = endian, \
}, \
}
extern const struct iio_event_spec lsm6dsm_fifo_flush_event;
#define ST_LSM6DSM_FLUSH_CHANNEL(device_type) \
{ \
.type = device_type, \
.modified = 0, \
.scan_index = -1, \
.indexed = -1, \
.event_spec = &lsm6dsm_fifo_flush_event,\
.num_event_specs = 1, \
}
#define ST_LSM6DSM_HWFIFO_ENABLED() \
IIO_DEVICE_ATTR(hwfifo_enabled, S_IWUSR | S_IRUGO, \
st_lsm6dsm_sysfs_get_hwfifo_enabled,\
st_lsm6dsm_sysfs_set_hwfifo_enabled, 0);
#define ST_LSM6DSM_HWFIFO_WATERMARK() \
IIO_DEVICE_ATTR(hwfifo_watermark, S_IWUSR | S_IRUGO, \
st_lsm6dsm_sysfs_get_hwfifo_watermark,\
st_lsm6dsm_sysfs_set_hwfifo_watermark, 0);
#define ST_LSM6DSM_HWFIFO_WATERMARK_MIN() \
IIO_DEVICE_ATTR(hwfifo_watermark_min, S_IRUGO, \
st_lsm6dsm_sysfs_get_hwfifo_watermark_min, NULL, 0);
#define ST_LSM6DSM_HWFIFO_WATERMARK_MAX() \
IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO, \
st_lsm6dsm_sysfs_get_hwfifo_watermark_max, NULL, 0);
#define ST_LSM6DSM_HWFIFO_FLUSH() \
IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, \
st_lsm6dsm_sysfs_flush_fifo, 0);
enum fifo_mode {
BYPASS = 0,
CONTINUOS,
};
struct st_lsm6dsm_transfer_buffer {
struct mutex buf_lock;
u8 rx_buf[ST_LSM6DSM_RX_MAX_LENGTH];
u8 tx_buf[ST_LSM6DSM_TX_MAX_LENGTH] ____cacheline_aligned;
};
struct lsm6dsm_out_decimation {
short decimator;
short num_samples;
};
struct lsm6dsm_fifo_output {
u8 sip;
int64_t deltatime;
int64_t deltatime_default;
int64_t timestamp;
int64_t timestamp_p;
short decimator;
short num_samples;
bool initialized;
};
/* struct lsm6dsm_data - common data for i2c or spi driver instance
* @name: pointer to the device name (i2c name or spi modalias).
* @enable_digfunc_mask: mask used to enable/disable hw digital functions.
* @enable_pedometer_mask: mask used to enable/disable hw pedometer function.
* @enable_sensorhub_mask: mask used to enable/disable sensor-hub feature.
* @irq_enable_fifo_mask: mask used to enable/disable fifo irq.
* @irq_enable_accel_ext_mask: mask used to enable/disable accel irq.
* @hw_odr: physical sensor odr expressed in Hz.
* @v_odr: requested sensor odr by userspace expressed in Hz.
* @hwfifo_enabled: is hwfifo enabled?
* @hwfifo_decimator: hwfifo decimator factor.
* @hwfifo_watermark: hwfifo watermark value.
* @samples_to_discard: samples to discard due to ODR switch.
* @nofifo_decimation: output status when fifo is disabled.
* @fifo_output: output status when fifo is enabled.
* @sensors_enabled: sensors enabled mask.
* @sensors_use_fifo: sensors use fifo mask.
* @accel_odr_dependency: odr dependency: accel, sensor-hub, dig-func.
* @accel_on: accel is going to be enabled during fifo odr switch?
* @magn_on: magn is going to be enabled during fifo odr switch?
* @odr_lock: mutex to avoid race condition during odr switch.
* @reset_steps: do I need to reset number of steps?
* @fifo_data: fifo data.
* @gyro_selftest_status: gyroscope selftest result.
* @accel_selftest_status: accelerometer selftest result.
* @irq: irq number.
* @timestamp: timestamp value from boot process.
*/
struct lsm6dsm_data {
const char *name;
u16 enable_digfunc_mask;
u16 enable_pedometer_mask;
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
u16 enable_sensorhub_mask;
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
u16 irq_enable_fifo_mask;
u16 irq_enable_accel_ext_mask;
unsigned int hw_odr[ST_INDIO_DEV_NUM + 1];
unsigned int v_odr[ST_INDIO_DEV_NUM + 1];
unsigned int trigger_odr;
bool hwfifo_enabled[ST_INDIO_DEV_NUM + 1];
u8 hwfifo_decimator[ST_INDIO_DEV_NUM + 1];
u16 hwfifo_watermark[ST_INDIO_DEV_NUM + 1];
u16 fifo_watermark;
u8 samples_to_discard[ST_INDIO_DEV_NUM + 1];
u8 samples_to_discard_2[ST_INDIO_DEV_NUM + 1];
struct lsm6dsm_out_decimation nofifo_decimation[ST_INDIO_DEV_NUM + 1];
struct lsm6dsm_fifo_output fifo_output[ST_INDIO_DEV_NUM + 1];
u16 sensors_enabled;
u16 sensors_use_fifo;
u64 num_steps;
int accel_odr_dependency[3];
bool accel_on;
bool magn_on;
enum fifo_mode fifo_status;
struct mutex odr_lock;
bool reset_steps;
u8 *fifo_data;
u8 accel_last_push[6];
u8 gyro_last_push[6];
u8 ext0_last_push[6];
int8_t gyro_selftest_status;
int8_t accel_selftest_status;
u8 drdy_reg;
int irq;
s64 timestamp;
int64_t fifo_enable_timestamp;
int64_t slower_counter;
uint8_t slower_id;
#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION
bool injection_mode;
s64 last_injection_timestamp;
u8 injection_odr;
#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */
struct work_struct data_work;
struct device *dev;
struct iio_dev *indio_dev[ST_INDIO_DEV_NUM + 1];
struct iio_trigger *trig[ST_INDIO_DEV_NUM + 1];
struct mutex bank_registers_lock;
struct mutex fifo_lock;
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
bool ext0_available;
int8_t ext0_selftest_status;
struct mutex i2c_transfer_lock;
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
const struct st_lsm6dsm_transfer_function *tf;
struct st_lsm6dsm_transfer_buffer tb;
};
struct st_lsm6dsm_transfer_function {
int (*write)(struct lsm6dsm_data *cdata,
u8 reg_addr, int len, u8 *data, bool b_lock);
int (*read)(struct lsm6dsm_data *cdata,
u8 reg_addr, int len, u8 *data, bool b_lock);
};
struct lsm6dsm_sensor_data {
struct lsm6dsm_data *cdata;
unsigned int c_gain[3];
u8 num_data_channels;
u8 sindex;
u8 data_out_reg;
};
int st_lsm6dsm_write_data_with_mask(struct lsm6dsm_data *cdata,
u8 reg_addr, u8 mask, u8 data, bool b_lock);
int st_lsm6dsm_push_data_with_timestamp(struct lsm6dsm_data *cdata,
u8 index, u8 *data, int64_t timestamp);
int st_lsm6dsm_common_probe(struct lsm6dsm_data *cdata, int irq);
void st_lsm6dsm_common_remove(struct lsm6dsm_data *cdata, int irq);
int st_lsm6dsm_set_enable(struct lsm6dsm_sensor_data *sdata, bool enable, bool buffer);
int st_lsm6dsm_set_fifo_mode(struct lsm6dsm_data *cdata, enum fifo_mode fm);
int st_lsm6dsm_enable_sensor_hub(struct lsm6dsm_data *cdata, bool enable,
enum st_mask_id id);
int lsm6dsm_read_output_data(struct lsm6dsm_data *cdata, int sindex, bool push);
int st_lsm6dsm_set_drdy_irq(struct lsm6dsm_sensor_data *sdata, bool state);
ssize_t st_lsm6dsm_sysfs_get_hwfifo_enabled(struct device *dev,
struct device_attribute *attr, char *buf);
ssize_t st_lsm6dsm_sysfs_set_hwfifo_enabled(struct device *dev,
struct device_attribute *attr, const char *buf, size_t size);
ssize_t st_lsm6dsm_sysfs_get_hwfifo_watermark(struct device *dev,
struct device_attribute *attr, char *buf);
ssize_t st_lsm6dsm_sysfs_set_hwfifo_watermark(struct device *dev,
struct device_attribute *attr, const char *buf, size_t size);
ssize_t st_lsm6dsm_sysfs_get_hwfifo_watermark_max(struct device *dev,
struct device_attribute *attr, char *buf);
ssize_t st_lsm6dsm_sysfs_get_hwfifo_watermark_min(struct device *dev,
struct device_attribute *attr, char *buf);
ssize_t st_lsm6dsm_sysfs_flush_fifo(struct device *dev,
struct device_attribute *attr, const char *buf, size_t size);
#ifdef CONFIG_IIO_BUFFER
int st_lsm6dsm_allocate_rings(struct lsm6dsm_data *cdata);
void st_lsm6dsm_deallocate_rings(struct lsm6dsm_data *cdata);
int st_lsm6dsm_trig_set_state(struct iio_trigger *trig, bool state);
int st_lsm6dsm_read_fifo(struct lsm6dsm_data *cdata, bool async);
#define ST_LSM6DSM_TRIGGER_SET_STATE (&st_lsm6dsm_trig_set_state)
#else /* CONFIG_IIO_BUFFER */
static inline int st_lsm6dsm_allocate_rings(struct lsm6dsm_data *cdata)
{
return 0;
}
static inline void st_lsm6dsm_deallocate_rings(struct lsm6dsm_data *cdata)
{
}
static inline int st_lsm6dsm_read_fifo(struct lsm6dsm_data *cdata, bool async)
{
return 0;
}
#define ST_LSM6DSM_TRIGGER_SET_STATE NULL
#endif /* CONFIG_IIO_BUFFER */
#ifdef CONFIG_IIO_TRIGGER
int st_lsm6dsm_allocate_triggers(struct lsm6dsm_data *cdata,
const struct iio_trigger_ops *trigger_ops);
void st_lsm6dsm_deallocate_triggers(struct lsm6dsm_data *cdata);
void st_lsm6dsm_flush_works(void);
#else /* CONFIG_IIO_TRIGGER */
static inline int st_lsm6dsm_allocate_triggers(struct lsm6dsm_data *cdata,
const struct iio_trigger_ops *trigger_ops, int irq)
{
return 0;
}
static inline void st_lsm6dsm_deallocate_triggers(struct lsm6dsm_data *cdata,
int irq)
{
return;
}
static inline void st_lsm6dsm_flush_works(void)
{
return;
}
#endif /* CONFIG_IIO_TRIGGER */
#ifdef CONFIG_PM
int st_lsm6dsm_common_suspend(struct lsm6dsm_data *cdata);
int st_lsm6dsm_common_resume(struct lsm6dsm_data *cdata);
#endif /* CONFIG_PM */
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
int st_lsm6dsm_write_embedded_registers(struct lsm6dsm_data *cdata,
u8 reg_addr, u8 *data, int len);
int st_lsm6dsm_i2c_master_probe(struct lsm6dsm_data *cdata);
int st_lsm6dsm_i2c_master_exit(struct lsm6dsm_data *cdata);
#else /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
static inline int st_lsm6dsm_i2c_master_probe(struct lsm6dsm_data *cdata)
{
return 0;
}
static inline int st_lsm6dsm_i2c_master_exit(struct lsm6dsm_data *cdata)
{
return 0;
}
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
#endif /* ST_LSM6DSM_H */

685
drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_buffer.c Normal file
View File

@ -0,0 +1,685 @@
// SPDX-License-Identifier: GPL-2.0-only
/*
* STMicroelectronics lsm6dsm buffer driver
*
* Copyright 2016 STMicroelectronics Inc.
*
* Denis Ciocca <denis.ciocca@st.com>
*/
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/slab.h>
#include <linux/stat.h>
#include <linux/interrupt.h>
#include <linux/i2c.h>
#include <linux/delay.h>
#include <linux/iio/iio.h>
#include <linux/iio/buffer.h>
#include <linux/iio/buffer_impl.h>
#include <linux/iio/trigger_consumer.h>
#include <linux/iio/triggered_buffer.h>
#include "st_lsm6dsm.h"
#define ST_LSM6DSM_FIFO_DIFF_L 0x3a
#define ST_LSM6DSM_FIFO_DIFF_MASK 0x07
#define ST_LSM6DSM_FIFO_DATA_OUT_L 0x3e
#define ST_LSM6DSM_FIFO_DATA_OVR 0x40
#define ST_LSM6DSM_FIFO_DATA_EMPTY 0x10
#define ST_LSM6DSM_STEP_MASK_64BIT (0xFFFFFFFFFFFF0000)
#define MIN_ID(a, b, c, d) (((a) < (b)) ? ((a == 0) ? \
(d) : (c)) : ((b == 0) ? \
(c) : (d)))
int st_lsm6dsm_push_data_with_timestamp(struct lsm6dsm_data *cdata,
u8 index, u8 *data, int64_t timestamp)
{
int i, n = 0;
struct iio_chan_spec const *chs = cdata->indio_dev[index]->channels;
uint16_t bfch, bfchs_out = 0, bfchs_in = 0;
struct lsm6dsm_sensor_data *sdata = iio_priv(cdata->indio_dev[index]);
u8 buff[ALIGN(ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE, sizeof(s64)) + sizeof(s64)];
if (timestamp <= cdata->fifo_output[index].timestamp_p)
return -EINVAL;
for (i = 0; i < sdata->num_data_channels; i++) {
bfch = chs[i].scan_type.storagebits >> 3;
if (test_bit(i, cdata->indio_dev[index]->active_scan_mask)) {
memcpy(&buff[bfchs_out], &data[bfchs_in], bfch);
n++;
bfchs_out += bfch;
}
bfchs_in += bfch;
}
iio_push_to_buffers_with_timestamp(cdata->indio_dev[index],
buff, timestamp);
cdata->fifo_output[index].timestamp_p = timestamp;
return 0;
}
static void st_lsm6dsm_parse_fifo_data(struct lsm6dsm_data *cdata,
u16 read_len, int64_t time_top, u16 num_pattern)
{
int err;
u16 fifo_offset = 0;
u8 gyro_sip, accel_sip;
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
u8 ext0_sip;
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
while (fifo_offset < read_len) {
gyro_sip = cdata->fifo_output[ST_MASK_ID_GYRO].sip;
accel_sip = cdata->fifo_output[ST_MASK_ID_ACCEL].sip;
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
ext0_sip = cdata->fifo_output[ST_MASK_ID_EXT0].sip;
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
do {
if (gyro_sip > 0) {
if (cdata->fifo_output[ST_MASK_ID_GYRO].timestamp == 0) {
if (cdata->slower_id == ST_MASK_ID_GYRO)
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = time_top -
(num_pattern * gyro_sip * cdata->fifo_output[ST_MASK_ID_GYRO].deltatime) - 300000;
else
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp = time_top -
(num_pattern * gyro_sip * cdata->fifo_output[ST_MASK_ID_GYRO].deltatime) - 300000 -
(cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_GYRO].deltatime);
} else
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp += cdata->fifo_output[ST_MASK_ID_GYRO].deltatime;
if (cdata->fifo_output[ST_MASK_ID_GYRO].timestamp > time_top) {
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp -= cdata->fifo_output[ST_MASK_ID_GYRO].deltatime;
cdata->samples_to_discard[ST_MASK_ID_GYRO] = 1;
}
if (cdata->samples_to_discard[ST_MASK_ID_GYRO] > 0)
cdata->samples_to_discard[ST_MASK_ID_GYRO]--;
else {
cdata->fifo_output[ST_MASK_ID_GYRO].num_samples++;
if (cdata->fifo_output[ST_MASK_ID_GYRO].num_samples >= cdata->fifo_output[ST_MASK_ID_GYRO].decimator) {
cdata->fifo_output[ST_MASK_ID_GYRO].num_samples = 0;
if (cdata->sensors_enabled & BIT(ST_MASK_ID_GYRO)) {
if (cdata->samples_to_discard_2[ST_MASK_ID_GYRO] == 0) {
err = st_lsm6dsm_push_data_with_timestamp(
cdata, ST_MASK_ID_GYRO,
&cdata->fifo_data[fifo_offset],
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp);
if (err >= 0)
cdata->fifo_output[ST_MASK_ID_GYRO].initialized = true;
memcpy(cdata->gyro_last_push, &cdata->fifo_data[fifo_offset], 6);
} else {
cdata->samples_to_discard_2[ST_MASK_ID_GYRO]--;
if (cdata->fifo_output[ST_MASK_ID_GYRO].initialized) {
err = st_lsm6dsm_push_data_with_timestamp(
cdata, ST_MASK_ID_GYRO,
cdata->gyro_last_push,
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp);
}
}
}
}
}
fifo_offset += ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE;
gyro_sip--;
}
if (accel_sip > 0) {
if (cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp == 0) {
if (cdata->slower_id == ST_MASK_ID_ACCEL)
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = time_top -
(num_pattern * accel_sip * cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime) - 300000;
else
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp = time_top -
(num_pattern * accel_sip * cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime) - 300000 -
(cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime);
} else
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp += cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime;
if (cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp > time_top) {
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp -= cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime;
cdata->samples_to_discard[ST_MASK_ID_ACCEL] = 1;
}
if (cdata->samples_to_discard[ST_MASK_ID_ACCEL] > 0)
cdata->samples_to_discard[ST_MASK_ID_ACCEL]--;
else {
cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples++;
if (cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples >= cdata->fifo_output[ST_MASK_ID_ACCEL].decimator) {
cdata->fifo_output[ST_MASK_ID_ACCEL].num_samples = 0;
if (cdata->sensors_enabled & BIT(ST_MASK_ID_ACCEL)) {
if (cdata->samples_to_discard_2[ST_MASK_ID_ACCEL] == 0) {
err = st_lsm6dsm_push_data_with_timestamp(
cdata, ST_MASK_ID_ACCEL,
&cdata->fifo_data[fifo_offset],
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp);
if (err >= 0)
cdata->fifo_output[ST_MASK_ID_ACCEL].initialized = true;
memcpy(cdata->accel_last_push, &cdata->fifo_data[fifo_offset], 6);
} else {
cdata->samples_to_discard_2[ST_MASK_ID_ACCEL]--;
if (cdata->fifo_output[ST_MASK_ID_ACCEL].initialized) {
err = st_lsm6dsm_push_data_with_timestamp(
cdata, ST_MASK_ID_ACCEL,
cdata->accel_last_push,
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp);
}
}
}
}
}
fifo_offset += ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE;
accel_sip--;
}
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
if (ext0_sip > 0) {
if (cdata->fifo_output[ST_MASK_ID_EXT0].timestamp == 0) {
if (cdata->slower_id == ST_MASK_ID_EXT0)
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = time_top -
(num_pattern * ext0_sip * cdata->fifo_output[ST_MASK_ID_EXT0].deltatime) - 300000;
else
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp = time_top -
(num_pattern * ext0_sip * cdata->fifo_output[ST_MASK_ID_EXT0].deltatime) - 300000 -
(cdata->fifo_output[cdata->slower_id].deltatime - cdata->fifo_output[ST_MASK_ID_EXT0].deltatime);
} else
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp += cdata->fifo_output[ST_MASK_ID_EXT0].deltatime;
if (cdata->fifo_output[ST_MASK_ID_EXT0].timestamp > time_top) {
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp -= cdata->fifo_output[ST_MASK_ID_EXT0].deltatime;
cdata->samples_to_discard[ST_MASK_ID_EXT0] = 1;
}
if (cdata->samples_to_discard[ST_MASK_ID_EXT0] > 0)
cdata->samples_to_discard[ST_MASK_ID_EXT0]--;
else {
cdata->fifo_output[ST_MASK_ID_EXT0].num_samples++;
if (cdata->fifo_output[ST_MASK_ID_EXT0].num_samples >= cdata->fifo_output[ST_MASK_ID_EXT0].decimator) {
cdata->fifo_output[ST_MASK_ID_EXT0].num_samples = 0;
if (cdata->sensors_enabled & BIT(ST_MASK_ID_EXT0)) {
if (cdata->samples_to_discard_2[ST_MASK_ID_EXT0] == 0) {
err = st_lsm6dsm_push_data_with_timestamp(
cdata, ST_MASK_ID_EXT0,
&cdata->fifo_data[fifo_offset],
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp);
if (err >= 0)
cdata->fifo_output[ST_MASK_ID_EXT0].initialized = true;
memcpy(cdata->ext0_last_push, &cdata->fifo_data[fifo_offset], 6);
} else {
cdata->samples_to_discard_2[ST_MASK_ID_EXT0]--;
if (cdata->fifo_output[ST_MASK_ID_EXT0].initialized) {
err = st_lsm6dsm_push_data_with_timestamp(
cdata, ST_MASK_ID_EXT0,
cdata->ext0_last_push,
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp);
}
}
}
}
}
fifo_offset += ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE;
ext0_sip--;
}
} while ((accel_sip > 0) || (gyro_sip > 0) || (ext0_sip > 0));
#else /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
} while ((accel_sip > 0) || (gyro_sip > 0));
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
}
}
int st_lsm6dsm_read_fifo(struct lsm6dsm_data *cdata, bool async)
{
int err;
u8 fifo_status[2];
#if (CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO > 0)
u16 data_remaining, data_to_read;
#endif /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */
u16 read_len = 0, byte_in_pattern, num_pattern;
int64_t temp_counter = 0, timestamp_diff, slower_deltatime;
err = cdata->tf->read(cdata, ST_LSM6DSM_FIFO_DIFF_L,
2, fifo_status, true);
if (err < 0)
return err;
timestamp_diff = iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]);
if (fifo_status[1] & ST_LSM6DSM_FIFO_DATA_OVR) {
st_lsm6dsm_set_fifo_mode(cdata, BYPASS);
st_lsm6dsm_set_fifo_mode(cdata, CONTINUOS);
dev_err(cdata->dev, "data fifo overrun, failed to read it.\n");
return -EINVAL;
}
if (fifo_status[1] & ST_LSM6DSM_FIFO_DATA_EMPTY)
return 0;
read_len = ((fifo_status[1] & ST_LSM6DSM_FIFO_DIFF_MASK) << 8) | fifo_status[0];
read_len *= ST_LSM6DSM_BYTE_FOR_CHANNEL;
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip +
cdata->fifo_output[ST_MASK_ID_GYRO].sip +
cdata->fifo_output[ST_MASK_ID_EXT0].sip) *
ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE;
#else /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip +
cdata->fifo_output[ST_MASK_ID_GYRO].sip) *
ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE;
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
if (byte_in_pattern == 0)
return 0;
num_pattern = read_len / byte_in_pattern;
read_len = (read_len / byte_in_pattern) * byte_in_pattern;
if (read_len == 0)
return 0;
#if (CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO == 0)
err = cdata->tf->read(cdata, ST_LSM6DSM_FIFO_DATA_OUT_L,
read_len, cdata->fifo_data, true);
if (err < 0)
return err;
#else /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */
data_remaining = read_len;
do {
if (data_remaining > CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO)
data_to_read = CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO;
else
data_to_read = data_remaining;
err = cdata->tf->read(cdata, ST_LSM6DSM_FIFO_DATA_OUT_L,
data_to_read,
&cdata->fifo_data[read_len - data_remaining], true);
if (err < 0)
return err;
data_remaining -= data_to_read;
} while (data_remaining > 0);
#endif /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */
cdata->slower_id = MIN_ID(cdata->fifo_output[ST_MASK_ID_GYRO].sip,
cdata->fifo_output[ST_MASK_ID_ACCEL].sip,
ST_MASK_ID_GYRO, ST_MASK_ID_ACCEL);
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
cdata->slower_id = MIN_ID(cdata->fifo_output[cdata->slower_id].sip,
cdata->fifo_output[ST_MASK_ID_EXT0].sip,
cdata->slower_id, ST_MASK_ID_EXT0);
#endif /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */
temp_counter = cdata->slower_counter;
cdata->slower_counter += (read_len / byte_in_pattern) * cdata->fifo_output[cdata->slower_id].sip;
if (async)
goto parse_fifo;
if (temp_counter > 0) {
slower_deltatime = div64_s64(timestamp_diff - cdata->fifo_enable_timestamp, cdata->slower_counter);
switch (cdata->slower_id) {
case ST_MASK_ID_ACCEL:
if (cdata->fifo_output[ST_MASK_ID_GYRO].sip != 0)
cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = div64_s64(slower_deltatime *
cdata->fifo_output[ST_MASK_ID_ACCEL].sip, cdata->fifo_output[ST_MASK_ID_GYRO].sip);
if (cdata->fifo_output[ST_MASK_ID_EXT0].sip != 0)
cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = div64_s64(slower_deltatime *
cdata->fifo_output[ST_MASK_ID_ACCEL].sip, cdata->fifo_output[ST_MASK_ID_EXT0].sip);
cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = slower_deltatime;
break;
case ST_MASK_ID_GYRO:
if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip != 0)
cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = div64_s64(slower_deltatime *
cdata->fifo_output[ST_MASK_ID_GYRO].sip, cdata->fifo_output[ST_MASK_ID_ACCEL].sip);
if (cdata->fifo_output[ST_MASK_ID_EXT0].sip != 0)
cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = div64_s64(slower_deltatime *
cdata->fifo_output[ST_MASK_ID_GYRO].sip, cdata->fifo_output[ST_MASK_ID_EXT0].sip);
cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = slower_deltatime;
break;
case ST_MASK_ID_EXT0:
if (cdata->fifo_output[ST_MASK_ID_ACCEL].sip != 0)
cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = div64_s64(slower_deltatime *
cdata->fifo_output[ST_MASK_ID_EXT0].sip, cdata->fifo_output[ST_MASK_ID_ACCEL].sip);
if (cdata->fifo_output[ST_MASK_ID_GYRO].sip != 0)
cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = div64_s64(slower_deltatime *
cdata->fifo_output[ST_MASK_ID_EXT0].sip, cdata->fifo_output[ST_MASK_ID_GYRO].sip);
cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = slower_deltatime;
break;
default:
break;
}
} else {
cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime = cdata->fifo_output[ST_MASK_ID_ACCEL].deltatime_default;
cdata->fifo_output[ST_MASK_ID_GYRO].deltatime = cdata->fifo_output[ST_MASK_ID_GYRO].deltatime_default;
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = cdata->fifo_output[ST_MASK_ID_EXT0].deltatime_default;
#endif /* CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO */
}
parse_fifo:
st_lsm6dsm_parse_fifo_data(cdata, read_len, timestamp_diff, num_pattern);
return 0;
}
int lsm6dsm_read_output_data(struct lsm6dsm_data *cdata, int sindex, bool push)
{
int err;
u8 data[6];
struct iio_dev *indio_dev = cdata->indio_dev[sindex];
struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev);
err = cdata->tf->read(cdata, sdata->data_out_reg,
ST_LSM6DSM_BYTE_FOR_CHANNEL * 3, data, true);
if (err < 0)
return err;
if (push)
st_lsm6dsm_push_data_with_timestamp(cdata, sindex,
data, cdata->timestamp);
return 0;
}
EXPORT_SYMBOL(lsm6dsm_read_output_data);
static irqreturn_t st_lsm6dsm_outdata_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t st_lsm6dsm_step_counter_trigger_handler(int irq, void *p)
{
int err;
u8 steps_data[2];
int64_t timestamp = 0;
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev);
u8 buff[ALIGN(ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE, sizeof(s64)) + sizeof(s64)];
if (!sdata->cdata->reset_steps) {
err = sdata->cdata->tf->read(sdata->cdata,
(u8)indio_dev->channels[0].address,
ST_LSM6DSM_BYTE_FOR_CHANNEL,
steps_data, true);
if (err < 0)
goto st_lsm6dsm_step_counter_done;
sdata->cdata->num_steps = (sdata->cdata->num_steps &
ST_LSM6DSM_STEP_MASK_64BIT) + *((u16 *)steps_data);
timestamp = sdata->cdata->timestamp;
} else {
sdata->cdata->num_steps = 0;
timestamp = iio_get_time_ns(indio_dev);
sdata->cdata->reset_steps = false;
}
memcpy(buff, (u8 *)&sdata->cdata->num_steps, sizeof(u64));
iio_push_to_buffers_with_timestamp(indio_dev, buff, timestamp);
st_lsm6dsm_step_counter_done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static irqreturn_t st_lsm6dsm_wrist_tilt_trigger_handler(int irq, void *p)
{
int err;
u8 wrist_tilt_gesture;
int64_t timestamp;
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev);
u8 buff[ALIGN(ST_LSM6DSM_FIFO_ELEMENT_LEN_BYTE, sizeof(s64)) + sizeof(s64)];
err = sdata->cdata->tf->read(sdata->cdata,
(u8)indio_dev->channels[0].address,
ST_LSM6DSM_BYTE_FOR_WRIST_TILT,
&wrist_tilt_gesture, true);
if (err < 0)
goto st_lsm6dsm_wrist_tilt_done;
buff[0] = wrist_tilt_gesture;
timestamp = sdata->cdata->timestamp;
iio_push_to_buffers_with_timestamp(indio_dev, buff, timestamp);
st_lsm6dsm_wrist_tilt_done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static inline irqreturn_t st_lsm6dsm_handler_empty(int irq, void *p)
{
return IRQ_HANDLED;
}
int st_lsm6dsm_trig_set_state(struct iio_trigger *trig, bool state)
{
return 0;
}
static int st_lsm6dsm_buffer_preenable(struct iio_dev *indio_dev)
{
#ifdef CONFIG_ST_LSM6DSM_XL_DATA_INJECTION
struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev);
if (sdata->cdata->injection_mode) {
switch (sdata->sindex) {
case ST_MASK_ID_ACCEL:
case ST_MASK_ID_GYRO:
return -EBUSY;
default:
break;
}
}
#endif /* CONFIG_ST_LSM6DSM_XL_DATA_INJECTION */
return 0;
}
static int st_lsm6dsm_buffer_postenable(struct iio_dev *indio_dev)
{
int err;
struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev);
sdata->cdata->fifo_output[sdata->sindex].initialized = false;
if ((sdata->cdata->hwfifo_enabled[sdata->sindex]) &&
(indio_dev->buffer->length < 2 * ST_LSM6DSM_MAX_FIFO_LENGHT))
return -EINVAL;
mutex_lock(&sdata->cdata->odr_lock);
err = st_lsm6dsm_set_enable(sdata, true, true);
if (err < 0) {
mutex_unlock(&sdata->cdata->odr_lock);
return err;
}
mutex_unlock(&sdata->cdata->odr_lock);
if (sdata->sindex == ST_MASK_ID_STEP_COUNTER)
iio_trigger_poll_chained(sdata->cdata->trig[ST_MASK_ID_STEP_COUNTER]);
return 0;
}
static int st_lsm6dsm_buffer_postdisable(struct iio_dev *indio_dev)
{
int err;
struct lsm6dsm_sensor_data *sdata = iio_priv(indio_dev);
mutex_lock(&sdata->cdata->odr_lock);
err = st_lsm6dsm_set_enable(sdata, false, true);
mutex_unlock(&sdata->cdata->odr_lock);
return err < 0 ? err : 0;
}
static const struct iio_buffer_setup_ops st_lsm6dsm_buffer_setup_ops = {
.preenable = &st_lsm6dsm_buffer_preenable,
.postenable = &st_lsm6dsm_buffer_postenable,
.postdisable = &st_lsm6dsm_buffer_postdisable,
};
int st_lsm6dsm_allocate_rings(struct lsm6dsm_data *cdata)
{
int err;
struct lsm6dsm_sensor_data *sdata;
sdata = iio_priv(cdata->indio_dev[ST_MASK_ID_ACCEL]);
err = iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_ACCEL],
NULL, &st_lsm6dsm_outdata_trigger_handler,
&st_lsm6dsm_buffer_setup_ops);
if (err < 0)
return err;
sdata = iio_priv(cdata->indio_dev[ST_MASK_ID_GYRO]);
err = iio_triggered_buffer_setup(cdata->indio_dev[ST_MASK_ID_GYRO],
NULL, &st_lsm6dsm_outdata_trigger_handler,
&st_lsm6dsm_buffer_setup_ops);
if (err < 0)
goto buffer_cleanup_accel;
err = iio_triggered_buffer_setup(
cdata->indio_dev[ST_MASK_ID_SIGN_MOTION],
&st_lsm6dsm_handler_empty, NULL,
&st_lsm6dsm_buffer_setup_ops);
if (err < 0)
goto buffer_cleanup_gyro;
err = iio_triggered_buffer_setup(
cdata->indio_dev[ST_MASK_ID_STEP_COUNTER],
NULL,
&st_lsm6dsm_step_counter_trigger_handler,
&st_lsm6dsm_buffer_setup_ops);
if (err < 0)
goto buffer_cleanup_sign_motion;
err = iio_triggered_buffer_setup(
cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR],
&st_lsm6dsm_handler_empty, NULL,
&st_lsm6dsm_buffer_setup_ops);
if (err < 0)
goto buffer_cleanup_step_counter;
err = iio_triggered_buffer_setup(
cdata->indio_dev[ST_MASK_ID_TILT],
&st_lsm6dsm_handler_empty, NULL,
&st_lsm6dsm_buffer_setup_ops);
if (err < 0)
goto buffer_cleanup_step_detector;
err = iio_triggered_buffer_setup(
cdata->indio_dev[ST_MASK_ID_WTILT],
NULL,
&st_lsm6dsm_wrist_tilt_trigger_handler,
&st_lsm6dsm_buffer_setup_ops);
if (err < 0)
goto buffer_cleanup_tilt;
err = iio_triggered_buffer_setup(
cdata->indio_dev[ST_MASK_ID_TAP],
&st_lsm6dsm_handler_empty, NULL,
&st_lsm6dsm_buffer_setup_ops);
if (err < 0)
goto buffer_cleanup_wtilt;
err = iio_triggered_buffer_setup(
cdata->indio_dev[ST_MASK_ID_TAP_TAP],
&st_lsm6dsm_handler_empty, NULL,
&st_lsm6dsm_buffer_setup_ops);
if (err < 0)
goto buffer_cleanup_tap;
return 0;
buffer_cleanup_tap:
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TAP]);
buffer_cleanup_wtilt:
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_WTILT]);
buffer_cleanup_tilt:
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TILT]);
buffer_cleanup_step_detector:
iio_triggered_buffer_cleanup(
cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]);
buffer_cleanup_step_counter:
iio_triggered_buffer_cleanup(
cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]);
buffer_cleanup_sign_motion:
iio_triggered_buffer_cleanup(
cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]);
buffer_cleanup_gyro:
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_GYRO]);
buffer_cleanup_accel:
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_ACCEL]);
return err;
}
void st_lsm6dsm_deallocate_rings(struct lsm6dsm_data *cdata)
{
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TAP_TAP]);
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TAP]);
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_WTILT]);
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_TILT]);
iio_triggered_buffer_cleanup(
cdata->indio_dev[ST_MASK_ID_STEP_DETECTOR]);
iio_triggered_buffer_cleanup(
cdata->indio_dev[ST_MASK_ID_STEP_COUNTER]);
iio_triggered_buffer_cleanup(
cdata->indio_dev[ST_MASK_ID_SIGN_MOTION]);
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_ACCEL]);
iio_triggered_buffer_cleanup(cdata->indio_dev[ST_MASK_ID_GYRO]);
}
MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
MODULE_DESCRIPTION("STMicroelectronics lsm6dsm buffer driver");
MODULE_LICENSE("GPL v2");

3439
drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_core.c Normal file
View File

File diff suppressed because it is too large Load Diff

177
drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_i2c.c Normal file
View File

@ -0,0 +1,177 @@
// SPDX-License-Identifier: GPL-2.0-only
/*
* STMicroelectronics lsm6dsm i2c driver
*
* Copyright 2016 STMicroelectronics Inc.
*
* Denis Ciocca <denis.ciocca@st.com>
*/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/i2c.h>
#include <linux/of.h>
#include <linux/iio/iio.h>
#include "st_lsm6dsm.h"
static int st_lsm6dsm_i2c_read(struct lsm6dsm_data *cdata,
u8 reg_addr, int len, u8 *data, bool b_lock)
{
int err = 0;
struct i2c_msg msg[2];
struct i2c_client *client = to_i2c_client(cdata->dev);
msg[0].addr = client->addr;
msg[0].flags = client->flags;
msg[0].len = 1;
msg[0].buf = &reg_addr;
msg[1].addr = client->addr;
msg[1].flags = client->flags | I2C_M_RD;
msg[1].len = len;
msg[1].buf = data;
if (b_lock) {
mutex_lock(&cdata->bank_registers_lock);
err = i2c_transfer(client->adapter, msg, 2);
mutex_unlock(&cdata->bank_registers_lock);
} else
err = i2c_transfer(client->adapter, msg, 2);
return err;
}
static int st_lsm6dsm_i2c_write(struct lsm6dsm_data *cdata,
u8 reg_addr, int len, u8 *data, bool b_lock)
{
int err = 0;
u8 send[len + 1];
struct i2c_msg msg;
struct i2c_client *client = to_i2c_client(cdata->dev);
send[0] = reg_addr;
memcpy(&send[1], data, len * sizeof(u8));
len++;
msg.addr = client->addr;
msg.flags = client->flags;
msg.len = len;
msg.buf = send;
if (b_lock) {
mutex_lock(&cdata->bank_registers_lock);
err = i2c_transfer(client->adapter, &msg, 1);
mutex_unlock(&cdata->bank_registers_lock);
} else
err = i2c_transfer(client->adapter, &msg, 1);
return err;
}
static const struct st_lsm6dsm_transfer_function st_lsm6dsm_tf_i2c = {
.write = st_lsm6dsm_i2c_write,
.read = st_lsm6dsm_i2c_read,
};
static int st_lsm6dsm_i2c_probe(struct i2c_client *client,
const struct i2c_device_id *id)
{
int err;
struct lsm6dsm_data *cdata;
cdata = kmalloc(sizeof(*cdata), GFP_KERNEL);
if (!cdata)
return -ENOMEM;
cdata->dev = &client->dev;
cdata->name = client->name;
i2c_set_clientdata(client, cdata);
cdata->tf = &st_lsm6dsm_tf_i2c;
err = st_lsm6dsm_common_probe(cdata, client->irq);
if (err < 0)
goto free_data;
return 0;
free_data:
kfree(cdata);
return err;
}
static int st_lsm6dsm_i2c_remove(struct i2c_client *client)
{
struct lsm6dsm_data *cdata = i2c_get_clientdata(client);
st_lsm6dsm_common_remove(cdata, client->irq);
kfree(cdata);
return 0;
}
#ifdef CONFIG_PM
static int __maybe_unused st_lsm6dsm_suspend(struct device *dev)
{
struct lsm6dsm_data *cdata = i2c_get_clientdata(to_i2c_client(dev));
return st_lsm6dsm_common_suspend(cdata);
}
static int __maybe_unused st_lsm6dsm_resume(struct device *dev)
{
struct lsm6dsm_data *cdata = i2c_get_clientdata(to_i2c_client(dev));
return st_lsm6dsm_common_resume(cdata);
}
static const struct dev_pm_ops st_lsm6dsm_pm_ops = {
SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsm_suspend, st_lsm6dsm_resume)
};
#define ST_LSM6DSM_PM_OPS (&st_lsm6dsm_pm_ops)
#else /* CONFIG_PM */
#define ST_LSM6DSM_PM_OPS NULL
#endif /* CONFIG_PM */
static const struct i2c_device_id st_lsm6dsm_id_table[] = {
{ LSM6DSM_DEV_NAME },
{ },
};
MODULE_DEVICE_TABLE(i2c, st_lsm6dsm_id_table);
#ifdef CONFIG_OF
static const struct of_device_id lsm6dsm_of_match[] = {
{
.compatible = "st,lsm6dsm",
.data = LSM6DSM_DEV_NAME,
},
{
.compatible = "st,lsm6dsl",
.data = LSM6DSL_DEV_NAME,
},
{},
};
MODULE_DEVICE_TABLE(of, lsm6dsm_of_match);
#else /* CONFIG_OF */
#define lsm6dsm_of_match NULL
#endif /* CONFIG_OF */
static struct i2c_driver st_lsm6dsm_driver = {
.driver = {
.owner = THIS_MODULE,
.name = "st-lsm6dsm-i2c",
.pm = ST_LSM6DSM_PM_OPS,
.of_match_table = of_match_ptr(lsm6dsm_of_match),
},
.probe = st_lsm6dsm_i2c_probe,
.remove = st_lsm6dsm_i2c_remove,
.id_table = st_lsm6dsm_id_table,
};
module_i2c_driver(st_lsm6dsm_driver);
MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
MODULE_DESCRIPTION("STMicroelectronics lsm6dsm i2c driver");
MODULE_LICENSE("GPL v2");

1764
drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_i2c_master.c Normal file
View File

File diff suppressed because it is too large Load Diff

198
drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_spi.c Normal file
View File

@ -0,0 +1,198 @@
// SPDX-License-Identifier: GPL-2.0-only
/*
* STMicroelectronics lsm6dsm spi driver
*
* Copyright 2016 STMicroelectronics Inc.
*
* Denis Ciocca <denis.ciocca@st.com>
*/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/spi/spi.h>
#include <linux/of.h>
#include <linux/iio/iio.h>
#include "st_lsm6dsm.h"
#define ST_SENSORS_SPI_READ 0x80
static int st_lsm6dsm_spi_read(struct lsm6dsm_data *cdata,
u8 reg_addr, int len, u8 *data, bool b_lock)
{
int err;
struct spi_transfer xfers[] = {
{
.tx_buf = cdata->tb.tx_buf,
.bits_per_word = 8,
.len = 1,
},
{
.rx_buf = cdata->tb.rx_buf,
.bits_per_word = 8,
.len = len,
}
};
if (b_lock)
mutex_lock(&cdata->bank_registers_lock);
mutex_lock(&cdata->tb.buf_lock);
cdata->tb.tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ;
err = spi_sync_transfer(to_spi_device(cdata->dev),
xfers, ARRAY_SIZE(xfers));
if (err)
goto acc_spi_read_error;
memcpy(data, cdata->tb.rx_buf, len*sizeof(u8));
mutex_unlock(&cdata->tb.buf_lock);
if (b_lock)
mutex_unlock(&cdata->bank_registers_lock);
return len;
acc_spi_read_error:
mutex_unlock(&cdata->tb.buf_lock);
if (b_lock)
mutex_unlock(&cdata->bank_registers_lock);
return err;
}
static int st_lsm6dsm_spi_write(struct lsm6dsm_data *cdata,
u8 reg_addr, int len, u8 *data, bool b_lock)
{
int err;
struct spi_transfer xfers = {
.tx_buf = cdata->tb.tx_buf,
.bits_per_word = 8,
.len = len + 1,
};
if (len >= ST_LSM6DSM_RX_MAX_LENGTH)
return -ENOMEM;
if (b_lock)
mutex_lock(&cdata->bank_registers_lock);
mutex_lock(&cdata->tb.buf_lock);
cdata->tb.tx_buf[0] = reg_addr;
memcpy(&cdata->tb.tx_buf[1], data, len);
err = spi_sync_transfer(to_spi_device(cdata->dev), &xfers, 1);
mutex_unlock(&cdata->tb.buf_lock);
if (b_lock)
mutex_unlock(&cdata->bank_registers_lock);
return err;
}
static const struct st_lsm6dsm_transfer_function st_lsm6dsm_tf_spi = {
.write = st_lsm6dsm_spi_write,
.read = st_lsm6dsm_spi_read,
};
static int st_lsm6dsm_spi_probe(struct spi_device *spi)
{
int err;
struct lsm6dsm_data *cdata;
cdata = kmalloc(sizeof(*cdata), GFP_KERNEL);
if (!cdata)
return -ENOMEM;
cdata->dev = &spi->dev;
cdata->name = spi->modalias;
spi_set_drvdata(spi, cdata);
cdata->tf = &st_lsm6dsm_tf_spi;
err = st_lsm6dsm_common_probe(cdata, spi->irq);
if (err < 0)
goto free_data;
return 0;
free_data:
kfree(cdata);
return err;
}
static int st_lsm6dsm_spi_remove(struct spi_device *spi)
{
struct lsm6dsm_data *cdata = spi_get_drvdata(spi);
st_lsm6dsm_common_remove(cdata, spi->irq);
kfree(cdata);
return 0;
}
#ifdef CONFIG_PM
static int __maybe_unused st_lsm6dsm_suspend(struct device *dev)
{
struct lsm6dsm_data *cdata = spi_get_drvdata(to_spi_device(dev));
return st_lsm6dsm_common_suspend(cdata);
}
static int __maybe_unused st_lsm6dsm_resume(struct device *dev)
{
struct lsm6dsm_data *cdata = spi_get_drvdata(to_spi_device(dev));
return st_lsm6dsm_common_resume(cdata);
}
static const struct dev_pm_ops st_lsm6dsm_pm_ops = {
SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsm_suspend, st_lsm6dsm_resume)
};
#define ST_LSM6DSM_PM_OPS (&st_lsm6dsm_pm_ops)
#else /* CONFIG_PM */
#define ST_LSM6DSM_PM_OPS NULL
#endif /* CONFIG_PM */
static const struct spi_device_id st_lsm6dsm_id_table[] = {
{ LSM6DSM_DEV_NAME },
{ },
};
MODULE_DEVICE_TABLE(spi, st_lsm6dsm_id_table);
#ifdef CONFIG_OF
static const struct of_device_id lsm6dsm_of_match[] = {
{
.compatible = "st,lsm6dsm",
.data = LSM6DSM_DEV_NAME,
},
{
.compatible = "st,lsm6dsl",
.data = LSM6DSL_DEV_NAME,
},
{},
};
MODULE_DEVICE_TABLE(of, lsm6dsm_of_match);
#else /* CONFIG_OF */
#define lsm6dsm_of_match NULL
#endif /* CONFIG_OF */
static struct spi_driver st_lsm6dsm_driver = {
.driver = {
.owner = THIS_MODULE,
.name = "st-lsm6dsm-spi",
.pm = ST_LSM6DSM_PM_OPS,
.of_match_table = of_match_ptr(lsm6dsm_of_match),
},
.probe = st_lsm6dsm_spi_probe,
.remove = st_lsm6dsm_spi_remove,
.id_table = st_lsm6dsm_id_table,
};
module_spi_driver(st_lsm6dsm_driver);
MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
MODULE_DESCRIPTION("STMicroelectronics lsm6dsm spi driver");
MODULE_LICENSE("GPL v2");

239
drivers/iio/stm/imu/st_lsm6dsm/st_lsm6dsm_trigger.c Normal file
View File

@ -0,0 +1,239 @@
// SPDX-License-Identifier: GPL-2.0-only
/*
* STMicroelectronics lsm6dsm trigger driver
*
* Copyright 2016 STMicroelectronics Inc.
*
* Denis Ciocca <denis.ciocca@st.com>
*/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/delay.h>
#include <linux/iio/iio.h>
#include <linux/iio/trigger.h>
#include <linux/interrupt.h>
#include <linux/iio/events.h>
#include "st_lsm6dsm.h"
#define ST_LSM6DSM_DIS_BIT 0x00
#define ST_LSM6DSM_SRC_FUNC_ADDR 0x53
#define ST_LSM6DSM_SRC2_FUNC_ADDR 0x54
#define ST_LSM6DSM_WRIST_TILT_IA 0x55
#define ST_LSM6DSM_WAKE_UP_SRC 0x1b
#define ST_LSM6DSM_TAP_SRC 0x1c
#define ST_LSM6DSM_STAP_EVENT BIT(5)
#define ST_LSM6DSM_DTAP_EVENT BIT(4)
#define ST_LSM6DSM_FIFO_DATA_AVL_ADDR 0x3b
#define ST_LSM6DSM_ACCEL_DATA_AVL_ADDR 0x1e
#define ST_LSM6DSM_ACCEL_DATA_AVL 0x01
#define ST_LSM6DSM_GYRO_DATA_AVL 0x02
#define ST_LSM6DSM_SRC_STEP_DETECTOR_DATA_AVL 0x10
#define ST_LSM6DSM_SRC_SIGN_MOTION_DATA_AVL 0x40
#define ST_LSM6DSM_SRC_TILT_DATA_AVL 0x20
#define ST_LSM6DSM_SRC_WTILT_DATA_AVL 0x01
#define ST_LSM6DSM_SRC_STEP_COUNTER_DATA_AVL 0x80
#define ST_LSM6DSM_SRC_STEP_COUNTER_DATA_OVR 0x08
#define ST_LSM6DSM_FIFO_DATA_AVL 0x80
#define ST_LSM6DSM_FIFO_DATA_OVR 0x40
static irqreturn_t lsm6dsm_irq_management(int irq, void *private)
{
int err;
bool push;
bool force_read_accel = false;
struct lsm6dsm_data *cdata = private;
u8 src_accel_gyro = 0, src_dig_func = 0, src2_dig_func = 0;
u8 src_tap_func = 0;
cdata->timestamp = iio_get_time_ns(cdata->indio_dev[ST_MASK_ID_ACCEL]);
if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) &
(BIT(ST_MASK_ID_ACCEL) | BIT(ST_MASK_ID_GYRO) |
BIT(ST_MASK_ID_EXT0))) {
err = cdata->tf->read(cdata, ST_LSM6DSM_ACCEL_DATA_AVL_ADDR,
1, &src_accel_gyro, true);
if (err < 0)
goto read_fifo_status;
if (src_accel_gyro & ST_LSM6DSM_ACCEL_DATA_AVL) {
#ifdef CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT
if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo)
& BIT(ST_MASK_ID_EXT0)) {
cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples++;
force_read_accel = true;
if ((cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples %
cdata->nofifo_decimation[ST_MASK_ID_EXT0].decimator) == 0) {
push = true;
cdata->nofifo_decimation[ST_MASK_ID_EXT0].num_samples = 0;
} else {
push = false;
}
lsm6dsm_read_output_data(cdata, ST_MASK_ID_EXT0, push);
}
#endif /* CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT */
if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) &
BIT(ST_MASK_ID_ACCEL)) {
cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples++;
if ((cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples %
cdata->nofifo_decimation[ST_MASK_ID_ACCEL].decimator) == 0) {
push = true;
cdata->nofifo_decimation[ST_MASK_ID_ACCEL].num_samples = 0;
} else {
push = false;
}
lsm6dsm_read_output_data(cdata, ST_MASK_ID_ACCEL, push);
} else {
if (force_read_accel)
lsm6dsm_read_output_data(cdata, ST_MASK_ID_ACCEL, false);
}
}
if (src_accel_gyro & ST_LSM6DSM_GYRO_DATA_AVL) {
if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & BIT(ST_MASK_ID_GYRO))
lsm6dsm_read_output_data(cdata, ST_MASK_ID_GYRO, true);
}
}
read_fifo_status:
if (cdata->sensors_use_fifo)
st_lsm6dsm_read_fifo(cdata, false);
err = cdata->tf->read(cdata, ST_LSM6DSM_SRC_FUNC_ADDR,
1, &src_dig_func, true);
if (err < 0)
goto exit_irq;
if ((src_dig_func & ST_LSM6DSM_SRC_STEP_DETECTOR_DATA_AVL) &&
(cdata->sensors_enabled & BIT(ST_MASK_ID_STEP_DETECTOR))) {
st_lsm6dsm_push_data_with_timestamp(cdata,
ST_MASK_ID_STEP_DETECTOR, NULL, cdata->timestamp);
}
if ((src_dig_func & ST_LSM6DSM_SRC_SIGN_MOTION_DATA_AVL) &&
(cdata->sensors_enabled & BIT(ST_MASK_ID_SIGN_MOTION))) {
iio_push_event(cdata->indio_dev[ST_MASK_ID_SIGN_MOTION],
IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION,
0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER),
cdata->timestamp);
}
if (src_dig_func & ST_LSM6DSM_SRC_STEP_COUNTER_DATA_OVR)
cdata->num_steps += (1 << 16);
if (src_dig_func & ST_LSM6DSM_SRC_STEP_COUNTER_DATA_AVL)
iio_trigger_poll_chained(cdata->trig[ST_MASK_ID_STEP_COUNTER]);
if ((src_dig_func & ST_LSM6DSM_SRC_TILT_DATA_AVL) &&
(cdata->sensors_enabled & BIT(ST_MASK_ID_TILT))) {
st_lsm6dsm_push_data_with_timestamp(cdata,
ST_MASK_ID_TILT, NULL, cdata->timestamp);
}
err = cdata->tf->read(cdata, ST_LSM6DSM_SRC2_FUNC_ADDR,
1, &src2_dig_func, true);
if (err < 0)
goto exit_irq;
if ((src2_dig_func & ST_LSM6DSM_SRC_WTILT_DATA_AVL) &&
(cdata->sensors_enabled & BIT(ST_MASK_ID_WTILT)))
iio_trigger_poll_chained(cdata->trig[ST_MASK_ID_WTILT]);
err = cdata->tf->read(cdata, ST_LSM6DSM_TAP_SRC,
1, &src_tap_func, true);
if (err < 0)
goto exit_irq;
if ((src_tap_func & ST_LSM6DSM_STAP_EVENT) &&
(cdata->sensors_enabled & BIT(ST_MASK_ID_TAP))) {
iio_push_event(cdata->indio_dev[ST_MASK_ID_TAP],
IIO_UNMOD_EVENT_CODE(IIO_TAP,
0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING),
cdata->timestamp);
}
if ((src_tap_func & ST_LSM6DSM_DTAP_EVENT) &&
(cdata->sensors_enabled & BIT(ST_MASK_ID_TAP_TAP))) {
iio_push_event(cdata->indio_dev[ST_MASK_ID_TAP_TAP],
IIO_UNMOD_EVENT_CODE(IIO_TAP_TAP,
0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING),
cdata->timestamp);
}
exit_irq:
return IRQ_HANDLED;
}
int st_lsm6dsm_allocate_triggers(struct lsm6dsm_data *cdata,
const struct iio_trigger_ops *trigger_ops)
{
int err, i, n;
for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
cdata->trig[i] = iio_trigger_alloc("%s-trigger",
cdata->indio_dev[i]->name);
if (!cdata->trig[i]) {
dev_err(cdata->dev,
"failed to allocate iio trigger.\n");
err = -ENOMEM;
goto deallocate_trigger;
}
iio_trigger_set_drvdata(cdata->trig[i], cdata->indio_dev[i]);
cdata->trig[i]->ops = trigger_ops;
cdata->trig[i]->dev.parent = cdata->dev;
}
err = request_threaded_irq(cdata->irq, NULL, lsm6dsm_irq_management,
IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
cdata->name, cdata);
if (err)
goto deallocate_trigger;
for (n = 0; n < ST_INDIO_DEV_NUM; n++) {
err = iio_trigger_register(cdata->trig[n]);
if (err < 0) {
dev_err(cdata->dev,
"failed to register iio trigger.\n");
goto free_irq;
}
cdata->indio_dev[n]->trig = cdata->trig[n];
}
return 0;
free_irq:
free_irq(cdata->irq, cdata);
for (n--; n >= 0; n--)
iio_trigger_unregister(cdata->trig[n]);
deallocate_trigger:
for (i--; i >= 0; i--)
iio_trigger_free(cdata->trig[i]);
return err;
}
EXPORT_SYMBOL(st_lsm6dsm_allocate_triggers);
void st_lsm6dsm_deallocate_triggers(struct lsm6dsm_data *cdata)
{
int i;
free_irq(cdata->irq, cdata);
for (i = 0; i < ST_INDIO_DEV_NUM; i++)
iio_trigger_unregister(cdata->trig[i]);
}
EXPORT_SYMBOL(st_lsm6dsm_deallocate_triggers);
MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
MODULE_DESCRIPTION("STMicroelectronics lsm6dsm trigger driver");
MODULE_LICENSE("GPL v2");

9
stm_iio_configs/lsm6dsm_defconfig Normal file
View File

@ -0,0 +1,9 @@
CONFIG_ST_LSM6DSM_IIO=m
CONFIG_ST_LSM6DSM_I2C_IIO=m
CONFIG_ST_LSM6DSM_SPI_IIO=m
CONFIG_ST_LSM6DSM_IIO_LIMIT_FIFO=0
CONFIG_ST_LSM6DSM_STEP_COUNTER_ON_DURING_SUSPEND=n
CONFIG_ST_LSM6DSM_IIO_MASTER_SUPPORT=y
CONFIG_ST_LSM6DSM_ENABLE_INTERNAL_PULLUP=y
CONFIG_ST_LSM6DSM_IIO_EXT0_LIS2MDL="LIS2MDL"
CONFIG_ST_LSM6DSM_XL_DATA_INJECTION=y