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

drivers:iio:stm:imu:Add support to LSM6DS3 STMEMS IMU

Browse Source 
Added support to LSM6DS3 STMEMS IMU.
This device was missing from the list of supported devices for
kernel 5.4 despite being supported in older version kernels.
This commit aligns device supported to previous kernel versions.

Signed-off-by: Mario Tesi <mario.tesi@st.com>
Change-Id: I679e66def879d17440e77d6df303263b0457a8d8
Reviewed-on: https://sczcxd1104.scz.st.com/gerrit/c/linux/stm-ldd-iio/+/630
Tested-by: CI STM MSD <aosp-ger@st.com>
Reviewed-by: Denis CIOCCA <denis.ciocca@st.com>
This commit is contained in:
Mario Tesi 2022-04-27 15:16:36 +02:00 committed by Denis CIOCCA
parent e2adc33a17
commit be8d9c7748
12 changed files with 6663 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

@ -12,6 +12,7 @@ source "drivers/iio/stm/imu/st_ism330dhcx/Kconfig"
source "drivers/iio/stm/imu/st_asm330lhh/Kconfig"
source "drivers/iio/stm/imu/st_imu68/Kconfig"
source "drivers/iio/stm/imu/st_ism330dlc/Kconfig"
source "drivers/iio/stm/imu/st_lsm6ds3/Kconfig"
source "drivers/iio/stm/imu/st_lsm6dsm/Kconfig"
source "drivers/iio/stm/imu/st_lsm6dsr/Kconfig"

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

@ -10,5 +10,6 @@ obj-y += st_ism330dhcx/
obj-y += st_asm330lhh/
obj-y += st_imu68/
obj-y += st_ism330dlc/
obj-y += st_lsm6ds3/
obj-y += st_lsm6dsm/
obj-y += st_lsm6dsr/

94
drivers/iio/stm/imu/st_lsm6ds3/Kconfig Normal file
View File

@ -0,0 +1,94 @@
#
# st-lsm6ds3 drivers for STMicroelectronics combo sensor
#
menuconfig ST_LSM6DS3_IIO
tristate "STMicroelectronics LSM6DS3/LSM6DS33 sensor"
depends on (I2C || SPI) && SYSFS
select IIO_BUFFER
select IIO_TRIGGERED_BUFFER
select ST_LSM6DS3_I2C_IIO if (I2C)
select ST_LSM6DS3_SPI_IIO if (SPI)
help
This driver supports LSM6DS3 and LSM6DS33 sensors.
It is a gyroscope/accelerometer combo device.
This driver can be built as a module. The module will be called
st-lsm6ds3.
if ST_LSM6DS3_IIO
config ST_LSM6DS3_I2C_IIO
tristate
depends on ST_LSM6DS3_IIO
depends on I2C
config ST_LSM6DS3_SPI_IIO
tristate
depends on ST_LSM6DS3_IIO
depends on SPI
config ST_LSM6DS3_IIO_LIMIT_FIFO
int "Limit fifo read lenght (#n byte)"
depends on ST_LSM6DS3_IIO
range 0 8192
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_LSM6DS3_STEP_COUNTER_ON_DURING_SUSPEND
bool "Keep Step counter on during suspend"
depends on ST_LSM6DS3_IIO
default n
help
During suspend step counter is kept on if enabled. Only interrupt
is disabled.
menuconfig ST_LSM6DS3_IIO_MASTER_SUPPORT
bool "I2C master controller"
depends on I2C && ST_LSM6DS3_IIO
default n
help
Added support for I2C master controller. Supported sensors up
to 4.
if ST_LSM6DS3_IIO_MASTER_SUPPORT
config ST_LSM6DS3_ENABLE_INTERNAL_PULLUP
bool "Enabled internals pull-up resistors"
default y
choice
prompt "External sensor 0"
default ST_LSM6DS3_IIO_EXT0_LIS3MDL
help
Choose the external sensor 0 connected to LSM6DS3.
config ST_LSM6DS3_IIO_EXT0_LIS3MDL
bool "LIS3MDL"
config ST_LSM6DS3_IIO_EXT0_AKM09911
bool "AKM09911"
config ST_LSM6DS3_IIO_EXT0_AKM09912
bool "AKM09912"
config ST_LSM6DS3_IIO_EXT0_AKM09916
bool "AKM09916"
config ST_LSM6DS3_IIO_EXT0_LPS22HB
bool "LPS22HB"
config ST_LSM6DS3_IIO_EXT0_LIS2MDL
bool "LIS2MDL"
endchoice
endif
config ST_LSM6DS3_XL_DATA_INJECTION
bool "Enable XL data injection support"
depends on ST_LSM6DS3_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_LSM6DS3_IIO

12
drivers/iio/stm/imu/st_lsm6ds3/Makefile Normal file
View File

@ -0,0 +1,12 @@
#
# Makefile for STMicroelectronics LSM6DS3 sensor.
#
obj-$(CONFIG_ST_LSM6DS3_IIO) += st_lsm6ds3.o
st_lsm6ds3-objs := st_lsm6ds3_core.o
obj-$(CONFIG_ST_LSM6DS3_I2C_IIO) += st_lsm6ds3_i2c.o
obj-$(CONFIG_ST_LSM6DS3_SPI_IIO) += st_lsm6ds3_spi.o
st_lsm6ds3-$(CONFIG_IIO_BUFFER) += st_lsm6ds3_buffer.o
st_lsm6ds3-$(CONFIG_IIO_TRIGGER) += st_lsm6ds3_trigger.o
st_lsm6ds3-$(CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT) += st_lsm6ds3_i2c_master.o

372
drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3.h Normal file
View File

@ -0,0 +1,372 @@
/*
* STMicroelectronics lsm6ds3 driver
*
* Copyright 2014-2016 STMicroelectronics Inc.
*
* Denis Ciocca <denis.ciocca@st.com>
*
* Licensed under the GPL-2.
*/
#ifndef ST_LSM6DS3_H
#define ST_LSM6DS3_H
#include <linux/types.h>
#include <linux/iio/trigger.h>
#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
#include <linux/i2c.h>
#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
#define LSM6DS3_DEV_NAME "lsm6ds3"
#define LSM6DS33_DEV_NAME "lsm6ds33"
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_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 6
#define ST_LSM6DS3_TX_MAX_LENGTH 12
#define ST_LSM6DS3_RX_MAX_LENGTH 8193
#define ST_LSM6DS3_BYTE_FOR_CHANNEL 2
#define ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE 6
#define ST_LSM6DS3_MAX_FIFO_SIZE 8192
#define ST_LSM6DS3_MAX_FIFO_THRESHOLD 1092
#define ST_LSM6DS3_MAX_FIFO_LENGHT (ST_LSM6DS3_MAX_FIFO_SIZE / \
ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE)
#define ST_LSM6DS3_SELFTEST_NA_MS "na"
#define ST_LSM6DS3_SELFTEST_FAIL_MS "fail"
#define ST_LSM6DS3_SELFTEST_PASS_MS "pass"
#define ST_LSM6DS3_WAKE_UP_SENSORS (BIT(ST_MASK_ID_SIGN_MOTION) | \
BIT(ST_MASK_ID_TILT))
#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
#define ST_LSM6DS3_NUM_CLIENTS 1
#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
#define ST_LSM6DS3_NUM_CLIENTS 0
#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
#define ST_LSM6DS3_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 lsm6ds3_fifo_flush_event;
#define ST_LSM6DS3_FLUSH_CHANNEL(device_type) \
{ \
.type = device_type, \
.modified = 0, \
.scan_index = -1, \
.indexed = -1, \
.event_spec = &lsm6ds3_fifo_flush_event,\
.num_event_specs = 1, \
}
#define ST_LSM6DS3_HWFIFO_ENABLED() \
IIO_DEVICE_ATTR(hwfifo_enabled, S_IWUSR | S_IRUGO, \
st_lsm6ds3_sysfs_get_hwfifo_enabled,\
st_lsm6ds3_sysfs_set_hwfifo_enabled, 0);
#define ST_LSM6DS3_HWFIFO_WATERMARK() \
IIO_DEVICE_ATTR(hwfifo_watermark, S_IWUSR | S_IRUGO, \
st_lsm6ds3_sysfs_get_hwfifo_watermark,\
st_lsm6ds3_sysfs_set_hwfifo_watermark, 0);
#define ST_LSM6DS3_HWFIFO_WATERMARK_MIN() \
IIO_DEVICE_ATTR(hwfifo_watermark_min, S_IRUGO, \
st_lsm6ds3_sysfs_get_hwfifo_watermark_min, NULL, 0);
#define ST_LSM6DS3_HWFIFO_WATERMARK_MAX() \
IIO_DEVICE_ATTR(hwfifo_watermark_max, S_IRUGO, \
st_lsm6ds3_sysfs_get_hwfifo_watermark_max, NULL, 0);
#define ST_LSM6DS3_HWFIFO_FLUSH() \
IIO_DEVICE_ATTR(hwfifo_flush, S_IWUSR, NULL, \
st_lsm6ds3_sysfs_flush_fifo, 0);
enum fifo_mode {
BYPASS = 0,
CONTINUOS,
};
struct st_lsm6ds3_transfer_buffer {
struct mutex buf_lock;
u8 rx_buf[ST_LSM6DS3_RX_MAX_LENGTH];
u8 tx_buf[ST_LSM6DS3_TX_MAX_LENGTH] ____cacheline_aligned;
};
struct lsm6ds3_out_decimation {
short decimator;
short num_samples;
};
struct lsm6ds3_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 lsm6ds3_data - common data for i2c or spi driver instance
* @name: pointer to the device name (i2c name or spi modalias).
* @spi_connection: set if driver probed by i2c or spi.
* @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?
* @sign_motion_event_ready: significan motion event is ready to be pushed.
* @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 lsm6ds3_data {
const char *name;
bool spi_connection;
u16 enable_digfunc_mask;
u16 enable_pedometer_mask;
#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
u16 enable_sensorhub_mask;
#endif /* CONFIG_ST_LSM6DS3_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 lsm6ds3_out_decimation nofifo_decimation[ST_INDIO_DEV_NUM + 1];
struct lsm6ds3_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;
bool sign_motion_event_ready;
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;
int irq;
s64 timestamp;
int64_t fifo_enable_timestamp;
int64_t slower_counter;
uint8_t slower_id;
#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION
bool injection_mode;
s64 last_injection_timestamp;
struct hrtimer injection_timer;
struct work_struct injection_work;
spinlock_t injection_spinlock;
u8 injection_data[30];
u8 injection_samples;
#endif /* CONFIG_ST_LSM6DS3_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_LSM6DS3_IIO_MASTER_SUPPORT
bool ext0_available;
int8_t ext0_selftest_status;
struct mutex i2c_transfer_lock;
#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
const struct st_lsm6ds3_transfer_function *tf;
struct st_lsm6ds3_transfer_buffer tb;
};
struct st_lsm6ds3_transfer_function {
int (*write)(struct lsm6ds3_data *cdata,
u8 reg_addr, int len, u8 *data, bool b_lock);
int (*read)(struct lsm6ds3_data *cdata,
u8 reg_addr, int len, u8 *data, bool b_lock);
};
struct lsm6ds3_sensor_data {
struct lsm6ds3_data *cdata;
unsigned int c_gain[3];
u8 num_data_channels;
u8 sindex;
u8 data_out_reg;
u8 *buffer_data;
};
int st_lsm6ds3_write_data_with_mask(struct lsm6ds3_data *cdata,
u8 reg_addr, u8 mask, u8 data, bool b_lock);
int st_lsm6ds3_push_data_with_timestamp(struct lsm6ds3_data *cdata,
u8 index, u8 *data, int64_t timestamp);
int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq);
void st_lsm6ds3_common_remove(struct lsm6ds3_data *cdata, int irq);
int st_lsm6ds3_set_enable(struct lsm6ds3_sensor_data *sdata, bool enable, bool buffer);
int st_lsm6ds3_set_fifo_mode(struct lsm6ds3_data *cdata, enum fifo_mode fm);
int st_lsm6ds3_enable_sensor_hub(struct lsm6ds3_data *cdata, bool enable,
enum st_mask_id id);
int lsm6ds3_read_output_data(struct lsm6ds3_data *cdata, int sindex, bool push);
int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state);
ssize_t st_lsm6ds3_sysfs_get_hwfifo_enabled(struct device *dev,
struct device_attribute *attr, char *buf);
ssize_t st_lsm6ds3_sysfs_set_hwfifo_enabled(struct device *dev,
struct device_attribute *attr, const char *buf, size_t size);
ssize_t st_lsm6ds3_sysfs_get_hwfifo_watermark(struct device *dev,
struct device_attribute *attr, char *buf);
ssize_t st_lsm6ds3_sysfs_set_hwfifo_watermark(struct device *dev,
struct device_attribute *attr, const char *buf, size_t size);
ssize_t st_lsm6ds3_sysfs_get_hwfifo_watermark_max(struct device *dev,
struct device_attribute *attr, char *buf);
ssize_t st_lsm6ds3_sysfs_get_hwfifo_watermark_min(struct device *dev,
struct device_attribute *attr, char *buf);
ssize_t st_lsm6ds3_sysfs_flush_fifo(struct device *dev,
struct device_attribute *attr, const char *buf, size_t size);
#ifdef CONFIG_IIO_BUFFER
int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata);
void st_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata);
int st_lsm6ds3_trig_set_state(struct iio_trigger *trig, bool state);
int st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool async);
#define ST_LSM6DS3_TRIGGER_SET_STATE (&st_lsm6ds3_trig_set_state)
#else /* CONFIG_IIO_BUFFER */
static inline int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata)
{
return 0;
}
static inline void st_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata)
{
}
static inline int st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool async)
{
return 0;
}
#define ST_LSM6DS3_TRIGGER_SET_STATE NULL
#endif /* CONFIG_IIO_BUFFER */
#ifdef CONFIG_IIO_TRIGGER
int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata,
const struct iio_trigger_ops *trigger_ops);
void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata);
void st_lsm6ds3_flush_works(void);
#else /* CONFIG_IIO_TRIGGER */
static inline int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata,
const struct iio_trigger_ops *trigger_ops, int irq)
{
return 0;
}
static inline void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata,
int irq)
{
return;
}
static inline void st_lsm6ds3_flush_works(void)
{
return;
}
#endif /* CONFIG_IIO_TRIGGER */
#ifdef CONFIG_PM
int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata);
int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata);
#endif /* CONFIG_PM */
#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
int st_lsm6ds3_write_embedded_registers(struct lsm6ds3_data *cdata,
u8 reg_addr, u8 *data, int len);
int st_lsm6ds3_i2c_master_probe(struct lsm6ds3_data *cdata);
int st_lsm6ds3_i2c_master_exit(struct lsm6ds3_data *cdata);
#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
static inline int st_lsm6ds3_i2c_master_probe(struct lsm6ds3_data *cdata)
{
return 0;
}
static inline int st_lsm6ds3_i2c_master_exit(struct lsm6ds3_data *cdata)
{
return 0;
}
#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
#endif /* ST_LSM6DS3_H */

653
drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_buffer.c Normal file
View File

@ -0,0 +1,653 @@
/*
* STMicroelectronics lsm6ds3 buffer driver
*
* Copyright 2014-2016 STMicroelectronics Inc.
*
* Denis Ciocca <denis.ciocca@st.com>
*
* Licensed under the GPL-2.
*/
#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/kfifo_buf.h>
#include <linux/iio/buffer.h>
#include <linux/iio/buffer_impl.h>
#include <linux/iio/trigger.h>
#include <linux/iio/trigger_consumer.h>
#include <linux/iio/triggered_buffer.h>
#include "st_lsm6ds3.h"
#define ST_LSM6DS3_ENABLE_AXIS 0x07
#define ST_LSM6DS3_FIFO_DIFF_L 0x3a
#define ST_LSM6DS3_FIFO_DIFF_MASK 0x0f
#define ST_LSM6DS3_FIFO_DATA_OUT_L 0x3e
#define ST_LSM6DS3_FIFO_DATA_OVR 0x40
#define ST_LSM6DS3_FIFO_DATA_EMPTY 0x10
#define ST_LSM6DS3_STEP_MASK_64BIT (0xFFFFFFFFFFFF0000)
#define MIN_ID(a, b, c, d) (((a) < (b)) ? ((a == 0) ? \
(d) : (c)) : ((b == 0) ? \
(c) : (d)))
int st_lsm6ds3_push_data_with_timestamp(struct lsm6ds3_data *cdata,
u8 index, u8 *data, int64_t timestamp)
{
size_t offset;
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 lsm6ds3_sensor_data *sdata = iio_priv(cdata->indio_dev[index]);
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(&sdata->buffer_data[bfchs_out],
&data[bfchs_in], bfch);
n++;
bfchs_out += bfch;
}
bfchs_in += bfch;
}
if (cdata->indio_dev[index]->scan_timestamp) {
offset = cdata->indio_dev[index]->scan_bytes / sizeof(s64) - 1;
((s64 *)sdata->buffer_data)[offset] = timestamp;
}
iio_push_to_buffers(cdata->indio_dev[index], sdata->buffer_data);
cdata->fifo_output[index].timestamp_p = timestamp;
return 0;
}
static void st_lsm6ds3_parse_fifo_data(struct lsm6ds3_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_LSM6DS3_IIO_MASTER_SUPPORT
u8 ext0_sip;
#endif /* CONFIG_ST_LSM6DS3_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_LSM6DS3_IIO_MASTER_SUPPORT
ext0_sip = cdata->fifo_output[ST_MASK_ID_EXT0].sip;
#endif /* CONFIG_ST_LSM6DS3_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_lsm6ds3_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_lsm6ds3_push_data_with_timestamp(
cdata, ST_MASK_ID_GYRO,
cdata->gyro_last_push,
cdata->fifo_output[ST_MASK_ID_GYRO].timestamp);
}
}
}
}
}
fifo_offset += ST_LSM6DS3_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_lsm6ds3_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_lsm6ds3_push_data_with_timestamp(
cdata, ST_MASK_ID_ACCEL,
cdata->accel_last_push,
cdata->fifo_output[ST_MASK_ID_ACCEL].timestamp);
}
}
}
}
}
fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE;
accel_sip--;
}
#ifdef CONFIG_ST_LSM6DS3_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_lsm6ds3_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_lsm6ds3_push_data_with_timestamp(
cdata, ST_MASK_ID_EXT0,
cdata->ext0_last_push,
cdata->fifo_output[ST_MASK_ID_EXT0].timestamp);
}
}
}
}
}
fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE;
ext0_sip--;
}
} while ((accel_sip > 0) || (gyro_sip > 0) || (ext0_sip > 0));
#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
} while ((accel_sip > 0) || (gyro_sip > 0));
#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
}
}
int st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool async)
{
int err;
u8 fifo_status[2];
#if (CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO > 0)
u16 data_remaining, data_to_read;
#endif /* CONFIG_ST_LSM6DS3_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_LSM6DS3_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_LSM6DS3_FIFO_DATA_OVR) {
st_lsm6ds3_set_fifo_mode(cdata, BYPASS);
st_lsm6ds3_set_fifo_mode(cdata, CONTINUOS);
dev_err(cdata->dev, "data fifo overrun, failed to read it.\n");
return -EINVAL;
}
if (fifo_status[1] & ST_LSM6DS3_FIFO_DATA_EMPTY)
return 0;
read_len = ((fifo_status[1] & ST_LSM6DS3_FIFO_DIFF_MASK) << 8) | fifo_status[0];
read_len *= ST_LSM6DS3_BYTE_FOR_CHANNEL;
#ifdef CONFIG_ST_LSM6DS3_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_LSM6DS3_FIFO_ELEMENT_LEN_BYTE;
#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
byte_in_pattern = (cdata->fifo_output[ST_MASK_ID_ACCEL].sip +
cdata->fifo_output[ST_MASK_ID_GYRO].sip) *
ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE;
#endif /* CONFIG_ST_LSM6DS3_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_LSM6DS3_IIO_LIMIT_FIFO == 0)
err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_OUT_L,
read_len, cdata->fifo_data, true);
if (err < 0)
return err;
#else /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */
data_remaining = read_len;
do {
if (data_remaining > CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO)
data_to_read = CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO;
else
data_to_read = data_remaining;
err = cdata->tf->read(cdata, ST_LSM6DS3_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_LSM6DS3_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_LSM6DS3_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_LSM6DS3_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_LSM6DS3_IIO_MASTER_SUPPORT
cdata->fifo_output[ST_MASK_ID_EXT0].deltatime = cdata->fifo_output[ST_MASK_ID_EXT0].deltatime_default;
#endif /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */
}
parse_fifo:
st_lsm6ds3_parse_fifo_data(cdata, read_len, timestamp_diff, num_pattern);
return 0;
}
int lsm6ds3_read_output_data(struct lsm6ds3_data *cdata, int sindex, bool push)
{
int err;
u8 data[6];
struct iio_dev *indio_dev = cdata->indio_dev[sindex];
struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
err = cdata->tf->read(cdata, sdata->data_out_reg,
ST_LSM6DS3_BYTE_FOR_CHANNEL * 3, data, true);
if (err < 0)
return err;
if (push)
st_lsm6ds3_push_data_with_timestamp(cdata, sindex,
data, cdata->timestamp);
return 0;
}
EXPORT_SYMBOL(lsm6ds3_read_output_data);
static irqreturn_t st_lsm6ds3_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_lsm6ds3_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 lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
if (!sdata->cdata->reset_steps) {
err = sdata->cdata->tf->read(sdata->cdata,
(u8)indio_dev->channels[0].address,
ST_LSM6DS3_BYTE_FOR_CHANNEL,
steps_data, true);
if (err < 0)
goto st_lsm6ds3_step_counter_done;
sdata->cdata->num_steps = (sdata->cdata->num_steps &
ST_LSM6DS3_STEP_MASK_64BIT) + *((u16 *)steps_data);
timestamp = sdata->cdata->timestamp;
} else {
sdata->cdata->num_steps = 0;
timestamp =
iio_get_time_ns(sdata->cdata->indio_dev[ST_MASK_ID_ACCEL]);
sdata->cdata->reset_steps = false;
}
memcpy(sdata->buffer_data, (u8 *)&sdata->cdata->num_steps, sizeof(u64));
if (indio_dev->scan_timestamp)
*(s64 *)((u8 *)sdata->buffer_data +
ALIGN(ST_LSM6DS3_BYTE_FOR_CHANNEL,
sizeof(s64))) = timestamp;
iio_push_to_buffers(indio_dev, sdata->buffer_data);
st_lsm6ds3_step_counter_done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static inline irqreturn_t st_lsm6ds3_handler_empty(int irq, void *p)
{
return IRQ_HANDLED;
}
int st_lsm6ds3_trig_set_state(struct iio_trigger *trig, bool state)
{
return 0;
}
static int st_lsm6ds3_buffer_preenable(struct iio_dev *indio_dev)
{
#ifdef CONFIG_ST_LSM6DS3_XL_DATA_INJECTION
struct lsm6ds3_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_LSM6DS3_XL_DATA_INJECTION */
return 0;
}
static int st_lsm6ds3_buffer_postenable(struct iio_dev *indio_dev)
{
int err;
struct lsm6ds3_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_LSM6DS3_MAX_FIFO_LENGHT))
return -EINVAL;
sdata->buffer_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
if (!sdata->buffer_data)
return -ENOMEM;
mutex_lock(&sdata->cdata->odr_lock);
err = st_lsm6ds3_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]);
if (sdata->sindex == ST_MASK_ID_SIGN_MOTION)
sdata->cdata->sign_motion_event_ready = true;
return 0;
}
static int st_lsm6ds3_buffer_postdisable(struct iio_dev *indio_dev)
{
int err;
struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
if (sdata->sindex == ST_MASK_ID_SIGN_MOTION)
sdata->cdata->sign_motion_event_ready = false;
mutex_lock(&sdata->cdata->odr_lock);
err = st_lsm6ds3_set_enable(sdata, false, true);
mutex_unlock(&sdata->cdata->odr_lock);
kfree(sdata->buffer_data);
return err < 0 ? err : 0;
}
static const struct iio_buffer_setup_ops st_lsm6ds3_buffer_setup_ops = {
.preenable = &st_lsm6ds3_buffer_preenable,
.postenable = &st_lsm6ds3_buffer_postenable,
.postdisable = &st_lsm6ds3_buffer_postdisable,
};
int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata)
{
int err;
struct lsm6ds3_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_lsm6ds3_outdata_trigger_handler,
&st_lsm6ds3_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_lsm6ds3_outdata_trigger_handler,
&st_lsm6ds3_buffer_setup_ops);
if (err < 0)
goto buffer_cleanup_accel;
err = iio_triggered_buffer_setup(
cdata->indio_dev[ST_MASK_ID_SIGN_MOTION],
&st_lsm6ds3_handler_empty, NULL,
&st_lsm6ds3_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_lsm6ds3_step_counter_trigger_handler,
&st_lsm6ds3_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_lsm6ds3_handler_empty, NULL,
&st_lsm6ds3_buffer_setup_ops);
if (err < 0)
goto buffer_cleanup_step_counter;
err = iio_triggered_buffer_setup(
cdata->indio_dev[ST_MASK_ID_TILT],
&st_lsm6ds3_handler_empty, NULL,
&st_lsm6ds3_buffer_setup_ops);
if (err < 0)
goto buffer_cleanup_step_detector;
return 0;
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_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata)
{
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 lsm6ds3 buffer driver");
MODULE_LICENSE("GPL v2");

3164
drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_core.c Normal file
View File

File diff suppressed because it is too large Load Diff

180
drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_i2c.c Normal file
View File

@ -0,0 +1,180 @@
/*
* STMicroelectronics lsm6ds3 i2c driver
*
* Copyright 2014-2016 STMicroelectronics Inc.
*
* Denis Ciocca <denis.ciocca@st.com>
*
* Licensed under the GPL-2.
*/
#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_lsm6ds3.h"
static int st_lsm6ds3_i2c_read(struct lsm6ds3_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_lsm6ds3_i2c_write(struct lsm6ds3_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_lsm6ds3_transfer_function st_lsm6ds3_tf_i2c = {
.write = st_lsm6ds3_i2c_write,
.read = st_lsm6ds3_i2c_read,
};
static int st_lsm6ds3_i2c_probe(struct i2c_client *client,
const struct i2c_device_id *id)
{
int err;
struct lsm6ds3_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->spi_connection = false;
cdata->tf = &st_lsm6ds3_tf_i2c;
err = st_lsm6ds3_common_probe(cdata, client->irq);
if (err < 0)
goto free_data;
return 0;
free_data:
kfree(cdata);
return err;
}
static int st_lsm6ds3_i2c_remove(struct i2c_client *client)
{
struct lsm6ds3_data *cdata = i2c_get_clientdata(client);
st_lsm6ds3_common_remove(cdata, client->irq);
kfree(cdata);
return 0;
}
#ifdef CONFIG_PM
static int st_lsm6ds3_suspend(struct device *dev)
{
struct lsm6ds3_data *cdata = i2c_get_clientdata(to_i2c_client(dev));
return st_lsm6ds3_common_suspend(cdata);
}
static int st_lsm6ds3_resume(struct device *dev)
{
struct lsm6ds3_data *cdata = i2c_get_clientdata(to_i2c_client(dev));
return st_lsm6ds3_common_resume(cdata);
}
static const struct dev_pm_ops st_lsm6ds3_pm_ops = {
SET_SYSTEM_SLEEP_PM_OPS(st_lsm6ds3_suspend, st_lsm6ds3_resume)
};
#define ST_LSM6DS3_PM_OPS (&st_lsm6ds3_pm_ops)
#else /* CONFIG_PM */
#define ST_LSM6DS3_PM_OPS NULL
#endif /* CONFIG_PM */
static const struct i2c_device_id st_lsm6ds3_id_table[] = {
{ LSM6DS3_DEV_NAME },
{ LSM6DS33_DEV_NAME },
{ },
};
MODULE_DEVICE_TABLE(i2c, st_lsm6ds3_id_table);
#ifdef CONFIG_OF
static const struct of_device_id lsm6ds3_of_match[] = {
{
.compatible = "st,lsm6ds3",
.data = LSM6DS3_DEV_NAME,
},
{
.compatible = "st,lsm6ds33",
.data = LSM6DS33_DEV_NAME,
},
{}
};
MODULE_DEVICE_TABLE(of, lsm6ds3_of_match);
#else /* CONFIG_OF */
#define lsm6ds3_of_match NULL
#endif /* CONFIG_OF */
static struct i2c_driver st_lsm6ds3_driver = {
.driver = {
.owner = THIS_MODULE,
.name = "st-lsm6ds3-i2c",
.pm = ST_LSM6DS3_PM_OPS,
.of_match_table = of_match_ptr(lsm6ds3_of_match),
},
.probe = st_lsm6ds3_i2c_probe,
.remove = st_lsm6ds3_i2c_remove,
.id_table = st_lsm6ds3_id_table,
};
module_i2c_driver(st_lsm6ds3_driver);
MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 i2c driver");
MODULE_LICENSE("GPL v2");

1771
drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_i2c_master.c Normal file
View File

File diff suppressed because it is too large Load Diff

201
drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_spi.c Normal file
View File

@ -0,0 +1,201 @@
/*
* STMicroelectronics lsm6ds3 spi driver
*
* Copyright 2014-2016 STMicroelectronics Inc.
*
* Denis Ciocca <denis.ciocca@st.com>
*
* Licensed under the GPL-2.
*/
#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_lsm6ds3.h"
#define ST_SENSORS_SPI_READ 0x80
static int st_lsm6ds3_spi_read(struct lsm6ds3_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_lsm6ds3_spi_write(struct lsm6ds3_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_LSM6DS3_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_lsm6ds3_transfer_function st_lsm6ds3_tf_spi = {
.write = st_lsm6ds3_spi_write,
.read = st_lsm6ds3_spi_read,
};
static int st_lsm6ds3_spi_probe(struct spi_device *spi)
{
int err;
struct lsm6ds3_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->spi_connection = true;
cdata->tf = &st_lsm6ds3_tf_spi;
err = st_lsm6ds3_common_probe(cdata, spi->irq);
if (err < 0)
goto free_data;
return 0;
free_data:
kfree(cdata);
return err;
}
static int st_lsm6ds3_spi_remove(struct spi_device *spi)
{
struct lsm6ds3_data *cdata = spi_get_drvdata(spi);
st_lsm6ds3_common_remove(cdata, spi->irq);
kfree(cdata);
return 0;
}
#ifdef CONFIG_PM
static int st_lsm6ds3_suspend(struct device *dev)
{
struct lsm6ds3_data *cdata = spi_get_drvdata(to_spi_device(dev));
return st_lsm6ds3_common_suspend(cdata);
}
static int st_lsm6ds3_resume(struct device *dev)
{
struct lsm6ds3_data *cdata = spi_get_drvdata(to_spi_device(dev));
return st_lsm6ds3_common_resume(cdata);
}
static const struct dev_pm_ops st_lsm6ds3_pm_ops = {
SET_SYSTEM_SLEEP_PM_OPS(st_lsm6ds3_suspend, st_lsm6ds3_resume)
};
#define ST_LSM6DS3_PM_OPS (&st_lsm6ds3_pm_ops)
#else /* CONFIG_PM */
#define ST_LSM6DS3_PM_OPS NULL
#endif /* CONFIG_PM */
static const struct spi_device_id st_lsm6ds3_id_table[] = {
{ LSM6DS3_DEV_NAME },
{ LSM6DS33_DEV_NAME },
{ },
};
MODULE_DEVICE_TABLE(spi, st_lsm6ds3_id_table);
#ifdef CONFIG_OF
static const struct of_device_id lsm6ds3_of_match[] = {
{
.compatible = "st,lsm6ds3",
.data = LSM6DS3_DEV_NAME,
},
{
.compatible = "st,lsm6ds33",
.data = LSM6DS33_DEV_NAME,
},
{}
};
MODULE_DEVICE_TABLE(of, lsm6ds3_of_match);
#else /* CONFIG_OF */
#define lsm6ds3_of_match NULL
#endif /* CONFIG_OF */
static struct spi_driver st_lsm6ds3_driver = {
.driver = {
.owner = THIS_MODULE,
.name = "st-lsm6ds3-spi",
.pm = ST_LSM6DS3_PM_OPS,
.of_match_table = of_match_ptr(lsm6ds3_of_match),
},
.probe = st_lsm6ds3_spi_probe,
.remove = st_lsm6ds3_spi_remove,
.id_table = st_lsm6ds3_id_table,
};
module_spi_driver(st_lsm6ds3_driver);
MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 spi driver");
MODULE_LICENSE("GPL v2");

205
drivers/iio/stm/imu/st_lsm6ds3/st_lsm6ds3_trigger.c Normal file
View File

@ -0,0 +1,205 @@
/*
* STMicroelectronics lsm6ds3 trigger driver
*
* Copyright 2014-2016 STMicroelectronics Inc.
*
* Denis Ciocca <denis.ciocca@st.com>
*
* Licensed under the GPL-2.
*/
#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_lsm6ds3.h"
#define ST_LSM6DS3_DIS_BIT 0x00
#define ST_LSM6DS3_SRC_FUNC_ADDR 0x53
#define ST_LSM6DS3_FIFO_DATA_AVL_ADDR 0x3b
#define ST_LSM6DS3_ACCEL_DATA_AVL_ADDR 0x1e
#define ST_LSM6DS3_ACCEL_DATA_AVL 0x01
#define ST_LSM6DS3_GYRO_DATA_AVL 0x02
#define ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL 0x10
#define ST_LSM6DS3_SRC_TILT_DATA_AVL 0x20
#define ST_LSM6DS3_SRC_STEP_COUNTER_DATA_AVL 0x80
#define ST_LSM6DS3_SRC_STEP_COUNTER_DATA_OVR 0x08
#define ST_LSM6DS3_FIFO_DATA_AVL 0x80
#define ST_LSM6DS3_FIFO_DATA_OVR 0x40
static irqreturn_t lsm6ds3_irq_management(int irq, void *private)
{
int err;
bool push;
bool force_read_accel = false;
struct lsm6ds3_data *cdata = private;
u8 src_accel_gyro = 0, src_dig_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_LSM6DS3_ACCEL_DATA_AVL_ADDR,
1, &src_accel_gyro, true);
if (err < 0)
goto read_fifo_status;
if (src_accel_gyro & ST_LSM6DS3_ACCEL_DATA_AVL) {
#ifdef CONFIG_ST_LSM6DS3_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;
lsm6ds3_read_output_data(cdata, ST_MASK_ID_EXT0, push);
}
#endif /* CONFIG_ST_LSM6DS3_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;
lsm6ds3_read_output_data(cdata, ST_MASK_ID_ACCEL, push);
} else {
if (force_read_accel)
lsm6ds3_read_output_data(cdata, ST_MASK_ID_ACCEL, false);
}
}
if (src_accel_gyro & ST_LSM6DS3_GYRO_DATA_AVL) {
if ((cdata->sensors_enabled & ~cdata->sensors_use_fifo) & BIT(ST_MASK_ID_GYRO))
lsm6ds3_read_output_data(cdata, ST_MASK_ID_GYRO, true);
}
}
read_fifo_status:
if (cdata->sensors_use_fifo)
st_lsm6ds3_read_fifo(cdata, false);
err = cdata->tf->read(cdata, ST_LSM6DS3_SRC_FUNC_ADDR,
1, &src_dig_func, true);
if (err < 0)
goto exit_irq;
if (src_dig_func & ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL) {
if (cdata->sensors_enabled & BIT(ST_MASK_ID_STEP_DETECTOR)) {
st_lsm6ds3_push_data_with_timestamp(cdata,
ST_MASK_ID_STEP_DETECTOR, NULL, cdata->timestamp);
}
if ((cdata->sign_motion_event_ready) &&
(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);
cdata->sign_motion_event_ready = false;
}
}
if (src_dig_func & ST_LSM6DS3_SRC_STEP_COUNTER_DATA_OVR)
cdata->num_steps += (1 << 16);
if (src_dig_func & ST_LSM6DS3_SRC_STEP_COUNTER_DATA_AVL)
iio_trigger_poll_chained(cdata->trig[ST_MASK_ID_STEP_COUNTER]);
if ((src_dig_func & ST_LSM6DS3_SRC_TILT_DATA_AVL) &&
(cdata->sensors_enabled & BIT(ST_MASK_ID_TILT))) {
st_lsm6ds3_push_data_with_timestamp(cdata,
ST_MASK_ID_TILT, NULL, cdata->timestamp);
}
exit_irq:
return IRQ_HANDLED;
}
int st_lsm6ds3_allocate_triggers(struct lsm6ds3_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, lsm6ds3_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_lsm6ds3_allocate_triggers);
void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_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_lsm6ds3_deallocate_triggers);
MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 trigger driver");
MODULE_LICENSE("GPL v2");

9
stm_iio_configs/lsm6ds3_defconfig Normal file
View File

@ -0,0 +1,9 @@
CONFIG_ST_LSM6DS3_IIO=m
CONFIG_ST_LSM6DS3_I2C_IIO=m
CONFIG_ST_LSM6DS3_SPI_IIO=m
CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO=0
CONFIG_ST_LSM6DS3_STEP_COUNTER_ON_DURING_SUSPEND=n
CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT=y
CONFIG_ST_LSM6DS3_ENABLE_INTERNAL_PULLUP=y
CONFIG_ST_LSM6DS3_IIO_EXT0_LIS2MDL="LIS2MDL"
CONFIG_ST_LSM6DS3_XL_DATA_INJECTION=y