diff options
author | Daniel Baumann <daniel.baumann@progress-linux.org> | 2024-05-06 01:02:30 +0000 |
---|---|---|
committer | Daniel Baumann <daniel.baumann@progress-linux.org> | 2024-05-06 01:02:30 +0000 |
commit | 76cb841cb886eef6b3bee341a2266c76578724ad (patch) | |
tree | f5892e5ba6cc11949952a6ce4ecbe6d516d6ce58 /drivers/iio/imu/st_lsm6dsx | |
parent | Initial commit. (diff) | |
download | linux-upstream.tar.xz linux-upstream.zip |
Adding upstream version 4.19.249.upstream/4.19.249upstream
Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>
Diffstat (limited to '')
-rw-r--r-- | drivers/iio/imu/st_lsm6dsx/Kconfig | 25 | ||||
-rw-r--r-- | drivers/iio/imu/st_lsm6dsx/Makefile | 5 | ||||
-rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 179 | ||||
-rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 585 | ||||
-rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 915 | ||||
-rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c | 92 | ||||
-rw-r--r-- | drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c | 92 |
7 files changed, 1893 insertions, 0 deletions
diff --git a/drivers/iio/imu/st_lsm6dsx/Kconfig b/drivers/iio/imu/st_lsm6dsx/Kconfig new file mode 100644 index 000000000..ccc817e17 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/Kconfig @@ -0,0 +1,25 @@ + +config IIO_ST_LSM6DSX + tristate "ST_LSM6DSx driver for STM 6-axis IMU MEMS sensors" + depends on (I2C || SPI) + select IIO_BUFFER + select IIO_KFIFO_BUF + select IIO_ST_LSM6DSX_I2C if (I2C) + select IIO_ST_LSM6DSX_SPI if (SPI_MASTER) + help + Say yes here to build support for STMicroelectronics LSM6DSx imu + sensor. Supported devices: lsm6ds3, lsm6ds3h, lsm6dsl, lsm6dsm, + ism330dlc + + To compile this driver as a module, choose M here: the module + will be called st_lsm6dsx. + +config IIO_ST_LSM6DSX_I2C + tristate + depends on IIO_ST_LSM6DSX + select REGMAP_I2C + +config IIO_ST_LSM6DSX_SPI + tristate + depends on IIO_ST_LSM6DSX + select REGMAP_SPI diff --git a/drivers/iio/imu/st_lsm6dsx/Makefile b/drivers/iio/imu/st_lsm6dsx/Makefile new file mode 100644 index 000000000..35919febe --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/Makefile @@ -0,0 +1,5 @@ +st_lsm6dsx-y := st_lsm6dsx_core.o st_lsm6dsx_buffer.o + +obj-$(CONFIG_IIO_ST_LSM6DSX) += st_lsm6dsx.o +obj-$(CONFIG_IIO_ST_LSM6DSX_I2C) += st_lsm6dsx_i2c.o +obj-$(CONFIG_IIO_ST_LSM6DSX_SPI) += st_lsm6dsx_spi.o diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h new file mode 100644 index 000000000..edcd83803 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h @@ -0,0 +1,179 @@ +/* + * STMicroelectronics st_lsm6dsx sensor driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi <lorenzo.bianconi@st.com> + * Denis Ciocca <denis.ciocca@st.com> + * + * Licensed under the GPL-2. + */ + +#ifndef ST_LSM6DSX_H +#define ST_LSM6DSX_H + +#include <linux/device.h> + +#define ST_LSM6DS3_DEV_NAME "lsm6ds3" +#define ST_LSM6DS3H_DEV_NAME "lsm6ds3h" +#define ST_LSM6DSL_DEV_NAME "lsm6dsl" +#define ST_LSM6DSM_DEV_NAME "lsm6dsm" +#define ST_ISM330DLC_DEV_NAME "ism330dlc" + +enum st_lsm6dsx_hw_id { + ST_LSM6DS3_ID, + ST_LSM6DS3H_ID, + ST_LSM6DSL_ID, + ST_LSM6DSM_ID, + ST_ISM330DLC_ID, + ST_LSM6DSX_MAX_ID, +}; + +#define ST_LSM6DSX_BUFF_SIZE 400 +#define ST_LSM6DSX_CHAN_SIZE 2 +#define ST_LSM6DSX_SAMPLE_SIZE 6 +#define ST_LSM6DSX_MAX_WORD_LEN ((32 / ST_LSM6DSX_SAMPLE_SIZE) * \ + ST_LSM6DSX_SAMPLE_SIZE) +#define ST_LSM6DSX_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask)) + +struct st_lsm6dsx_reg { + u8 addr; + u8 mask; +}; + +/** + * struct st_lsm6dsx_fifo_ops - ST IMU FIFO settings + * @fifo_th: FIFO threshold register info (addr + mask). + * @fifo_diff: FIFO diff status register info (addr + mask). + * @th_wl: FIFO threshold word length. + */ +struct st_lsm6dsx_fifo_ops { + struct { + u8 addr; + u16 mask; + } fifo_th; + struct { + u8 addr; + u16 mask; + } fifo_diff; + u8 th_wl; +}; + +/** + * struct st_lsm6dsx_hw_ts_settings - ST IMU hw timer settings + * @timer_en: Hw timer enable register info (addr + mask). + * @hr_timer: Hw timer resolution register info (addr + mask). + * @fifo_en: Hw timer FIFO enable register info (addr + mask). + * @decimator: Hw timer FIFO decimator register info (addr + mask). + */ +struct st_lsm6dsx_hw_ts_settings { + struct st_lsm6dsx_reg timer_en; + struct st_lsm6dsx_reg hr_timer; + struct st_lsm6dsx_reg fifo_en; + struct st_lsm6dsx_reg decimator; +}; + +/** + * struct st_lsm6dsx_settings - ST IMU sensor settings + * @wai: Sensor WhoAmI default value. + * @max_fifo_size: Sensor max fifo length in FIFO words. + * @id: List of hw id supported by the driver configuration. + * @decimator: List of decimator register info (addr + mask). + * @fifo_ops: Sensor hw FIFO parameters. + * @ts_settings: Hw timer related settings. + */ +struct st_lsm6dsx_settings { + u8 wai; + u16 max_fifo_size; + enum st_lsm6dsx_hw_id id[ST_LSM6DSX_MAX_ID]; + struct st_lsm6dsx_reg decimator[ST_LSM6DSX_MAX_ID]; + struct st_lsm6dsx_fifo_ops fifo_ops; + struct st_lsm6dsx_hw_ts_settings ts_settings; +}; + +enum st_lsm6dsx_sensor_id { + ST_LSM6DSX_ID_ACC, + ST_LSM6DSX_ID_GYRO, + ST_LSM6DSX_ID_MAX, +}; + +enum st_lsm6dsx_fifo_mode { + ST_LSM6DSX_FIFO_BYPASS = 0x0, + ST_LSM6DSX_FIFO_CONT = 0x6, +}; + +/** + * struct st_lsm6dsx_sensor - ST IMU sensor instance + * @name: Sensor name. + * @id: Sensor identifier. + * @hw: Pointer to instance of struct st_lsm6dsx_hw. + * @gain: Configured sensor sensitivity. + * @odr: Output data rate of the sensor [Hz]. + * @watermark: Sensor watermark level. + * @sip: Number of samples in a given pattern. + * @decimator: FIFO decimation factor. + * @ts_ref: Sensor timestamp reference for hw one. + */ +struct st_lsm6dsx_sensor { + char name[32]; + enum st_lsm6dsx_sensor_id id; + struct st_lsm6dsx_hw *hw; + + u32 gain; + u16 odr; + + u16 watermark; + u8 sip; + u8 decimator; + s64 ts_ref; +}; + +/** + * struct st_lsm6dsx_hw - ST IMU MEMS hw instance + * @dev: Pointer to instance of struct device (I2C or SPI). + * @regmap: Register map of the device. + * @irq: Device interrupt line (I2C or SPI). + * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO. + * @conf_lock: Mutex to prevent concurrent FIFO configuration update. + * @fifo_mode: FIFO operating mode supported by the device. + * @enable_mask: Enabled sensor bitmask. + * @ts_sip: Total number of timestamp samples in a given pattern. + * @sip: Total number of samples (acc/gyro/ts) in a given pattern. + * @buff: Device read buffer. + * @iio_devs: Pointers to acc/gyro iio_dev instances. + * @settings: Pointer to the specific sensor settings in use. + */ +struct st_lsm6dsx_hw { + struct device *dev; + struct regmap *regmap; + int irq; + + struct mutex fifo_lock; + struct mutex conf_lock; + + enum st_lsm6dsx_fifo_mode fifo_mode; + u8 enable_mask; + u8 ts_sip; + u8 sip; + + u8 *buff; + + struct iio_dev *iio_devs[ST_LSM6DSX_ID_MAX]; + + const struct st_lsm6dsx_settings *settings; +}; + +extern const struct dev_pm_ops st_lsm6dsx_pm_ops; + +int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name, + struct regmap *regmap); +int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor); +int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor); +int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw); +int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, + u16 watermark); +int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw); +int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw, + enum st_lsm6dsx_fifo_mode fifo_mode); + +#endif /* ST_LSM6DSX_H */ diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c new file mode 100644 index 000000000..4d89de0be --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c @@ -0,0 +1,585 @@ +/* + * STMicroelectronics st_lsm6dsx FIFO buffer library driver + * + * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC: The FIFO buffer can be + * configured to store data from gyroscope and accelerometer. Samples are + * queued without any tag according to a specific pattern based on + * 'FIFO data sets' (6 bytes each): + * - 1st data set is reserved for gyroscope data + * - 2nd data set is reserved for accelerometer data + * The FIFO pattern changes depending on the ODRs and decimation factors + * assigned to the FIFO data sets. The first sequence of data stored in FIFO + * buffer contains the data of all the enabled FIFO data sets + * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the + * value of the decimation factor and ODR set for each FIFO data set. + * FIFO supported modes: + * - BYPASS: FIFO disabled + * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index + * restarts from the beginning and the oldest sample is overwritten + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi <lorenzo.bianconi@st.com> + * Denis Ciocca <denis.ciocca@st.com> + * + * Licensed under the GPL-2. + */ +#include <linux/module.h> +#include <linux/interrupt.h> +#include <linux/irq.h> +#include <linux/iio/kfifo_buf.h> +#include <linux/iio/iio.h> +#include <linux/iio/buffer.h> +#include <linux/regmap.h> +#include <linux/bitfield.h> + +#include <linux/platform_data/st_sensors_pdata.h> + +#include "st_lsm6dsx.h" + +#define ST_LSM6DSX_REG_HLACTIVE_ADDR 0x12 +#define ST_LSM6DSX_REG_HLACTIVE_MASK BIT(5) +#define ST_LSM6DSX_REG_PP_OD_ADDR 0x12 +#define ST_LSM6DSX_REG_PP_OD_MASK BIT(4) +#define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a +#define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0) +#define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3) +#define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12) +#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e +#define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42 + +#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08 + +#define ST_LSM6DSX_TS_SENSITIVITY 25000UL /* 25us */ +#define ST_LSM6DSX_TS_RESET_VAL 0xaa + +struct st_lsm6dsx_decimator_entry { + u8 decimator; + u8 val; +}; + +static const +struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_table[] = { + { 0, 0x0 }, + { 1, 0x1 }, + { 2, 0x2 }, + { 3, 0x3 }, + { 4, 0x4 }, + { 8, 0x5 }, + { 16, 0x6 }, + { 32, 0x7 }, +}; + +static int st_lsm6dsx_get_decimator_val(u8 val) +{ + const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table); + int i; + + for (i = 0; i < max_size; i++) + if (st_lsm6dsx_decimator_table[i].decimator == val) + break; + + return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val; +} + +static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw, + u16 *max_odr, u16 *min_odr) +{ + struct st_lsm6dsx_sensor *sensor; + int i; + + *max_odr = 0, *min_odr = ~0; + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + *max_odr = max_t(u16, *max_odr, sensor->odr); + *min_odr = min_t(u16, *min_odr, sensor->odr); + } +} + +static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw) +{ + u16 max_odr, min_odr, sip = 0, ts_sip = 0; + const struct st_lsm6dsx_reg *ts_dec_reg; + struct st_lsm6dsx_sensor *sensor; + int err = 0, i; + u8 data; + + st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr); + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + const struct st_lsm6dsx_reg *dec_reg; + + sensor = iio_priv(hw->iio_devs[i]); + /* update fifo decimators and sample in pattern */ + if (hw->enable_mask & BIT(sensor->id)) { + sensor->sip = sensor->odr / min_odr; + sensor->decimator = max_odr / sensor->odr; + data = st_lsm6dsx_get_decimator_val(sensor->decimator); + } else { + sensor->sip = 0; + sensor->decimator = 0; + data = 0; + } + ts_sip = max_t(u16, ts_sip, sensor->sip); + + dec_reg = &hw->settings->decimator[sensor->id]; + if (dec_reg->addr) { + int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask); + + err = regmap_update_bits(hw->regmap, dec_reg->addr, + dec_reg->mask, val); + if (err < 0) + return err; + } + sip += sensor->sip; + } + hw->sip = sip + ts_sip; + hw->ts_sip = ts_sip; + + /* + * update hw ts decimator if necessary. Decimator for hw timestamp + * is always 1 or 0 in order to have a ts sample for each data + * sample in FIFO + */ + ts_dec_reg = &hw->settings->ts_settings.decimator; + if (ts_dec_reg->addr) { + int val, ts_dec = !!hw->ts_sip; + + val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask); + err = regmap_update_bits(hw->regmap, ts_dec_reg->addr, + ts_dec_reg->mask, val); + } + return err; +} + +int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw, + enum st_lsm6dsx_fifo_mode fifo_mode) +{ + int err; + + err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR, + ST_LSM6DSX_FIFO_MODE_MASK, + FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, + fifo_mode)); + if (err < 0) + return err; + + hw->fifo_mode = fifo_mode; + + return 0; +} + +static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor, + bool enable) +{ + struct st_lsm6dsx_hw *hw = sensor->hw; + u8 data; + + data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0; + return regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR, + ST_LSM6DSX_FIFO_ODR_MASK, + FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK, data)); +} + +int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark) +{ + u16 fifo_watermark = ~0, cur_watermark, fifo_th_mask; + struct st_lsm6dsx_hw *hw = sensor->hw; + struct st_lsm6dsx_sensor *cur_sensor; + int i, err, data; + __le16 wdata; + + if (!hw->sip) + return 0; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + cur_sensor = iio_priv(hw->iio_devs[i]); + + if (!(hw->enable_mask & BIT(cur_sensor->id))) + continue; + + cur_watermark = (cur_sensor == sensor) ? watermark + : cur_sensor->watermark; + + fifo_watermark = min_t(u16, fifo_watermark, cur_watermark); + } + + fifo_watermark = max_t(u16, fifo_watermark, hw->sip); + fifo_watermark = (fifo_watermark / hw->sip) * hw->sip; + fifo_watermark = fifo_watermark * hw->settings->fifo_ops.th_wl; + + err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1, + &data); + if (err < 0) + return err; + + fifo_th_mask = hw->settings->fifo_ops.fifo_th.mask; + fifo_watermark = ((data << 8) & ~fifo_th_mask) | + (fifo_watermark & fifo_th_mask); + + wdata = cpu_to_le16(fifo_watermark); + return regmap_bulk_write(hw->regmap, + hw->settings->fifo_ops.fifo_th.addr, + &wdata, sizeof(wdata)); +} + +static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw) +{ + struct st_lsm6dsx_sensor *sensor; + int i, err; + + /* reset hw ts counter */ + err = regmap_write(hw->regmap, ST_LSM6DSX_REG_TS_RESET_ADDR, + ST_LSM6DSX_TS_RESET_VAL); + if (err < 0) + return err; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + /* + * store enable buffer timestamp as reference for + * hw timestamp + */ + sensor->ts_ref = iio_get_time_ns(hw->iio_devs[i]); + } + return 0; +} + +/* + * Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN in order to avoid + * a kmalloc for each bus access + */ +static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 *data, + unsigned int data_len) +{ + unsigned int word_len, read_len = 0; + int err; + + while (read_len < data_len) { + word_len = min_t(unsigned int, data_len - read_len, + ST_LSM6DSX_MAX_WORD_LEN); + err = regmap_bulk_read(hw->regmap, + ST_LSM6DSX_REG_FIFO_OUTL_ADDR, + data + read_len, word_len); + if (err < 0) + return err; + read_len += word_len; + } + return 0; +} + +#define ST_LSM6DSX_IIO_BUFF_SIZE (ALIGN(ST_LSM6DSX_SAMPLE_SIZE, \ + sizeof(s64)) + sizeof(s64)) +/** + * st_lsm6dsx_read_fifo() - hw FIFO read routine + * @hw: Pointer to instance of struct st_lsm6dsx_hw. + * + * Read samples from the hw FIFO and push them to IIO buffers. + * + * Return: Number of bytes read from the FIFO + */ +static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) +{ + u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE; + u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask; + int err, acc_sip, gyro_sip, ts_sip, read_len, offset; + struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor; + u8 gyro_buff[ST_LSM6DSX_IIO_BUFF_SIZE]; + u8 acc_buff[ST_LSM6DSX_IIO_BUFF_SIZE]; + bool reset_ts = false; + __le16 fifo_status; + s64 ts = 0; + + err = regmap_bulk_read(hw->regmap, + hw->settings->fifo_ops.fifo_diff.addr, + &fifo_status, sizeof(fifo_status)); + if (err < 0) { + dev_err(hw->dev, "failed to read fifo status (err=%d)\n", + err); + return err; + } + + if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK)) + return 0; + + fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) * + ST_LSM6DSX_CHAN_SIZE; + fifo_len = (fifo_len / pattern_len) * pattern_len; + + acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]); + gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]); + + for (read_len = 0; read_len < fifo_len; read_len += pattern_len) { + err = st_lsm6dsx_read_block(hw, hw->buff, pattern_len); + if (err < 0) { + dev_err(hw->dev, + "failed to read pattern from fifo (err=%d)\n", + err); + return err; + } + + /* + * Data are written to the FIFO with a specific pattern + * depending on the configured ODRs. The first sequence of data + * stored in FIFO contains the data of all enabled sensors + * (e.g. Gx, Gy, Gz, Ax, Ay, Az, Ts), then data are repeated + * depending on the value of the decimation factor set for each + * sensor. + * + * Supposing the FIFO is storing data from gyroscope and + * accelerometer at different ODRs: + * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz + * Since the gyroscope ODR is twice the accelerometer one, the + * following pattern is repeated every 9 samples: + * - Gx, Gy, Gz, Ax, Ay, Az, Ts, Gx, Gy, Gz, Ts, Gx, .. + */ + gyro_sip = gyro_sensor->sip; + acc_sip = acc_sensor->sip; + ts_sip = hw->ts_sip; + offset = 0; + + while (acc_sip > 0 || gyro_sip > 0) { + if (gyro_sip > 0) { + memcpy(gyro_buff, &hw->buff[offset], + ST_LSM6DSX_SAMPLE_SIZE); + offset += ST_LSM6DSX_SAMPLE_SIZE; + } + if (acc_sip > 0) { + memcpy(acc_buff, &hw->buff[offset], + ST_LSM6DSX_SAMPLE_SIZE); + offset += ST_LSM6DSX_SAMPLE_SIZE; + } + + if (ts_sip-- > 0) { + u8 data[ST_LSM6DSX_SAMPLE_SIZE]; + + memcpy(data, &hw->buff[offset], sizeof(data)); + /* + * hw timestamp is 3B long and it is stored + * in FIFO using 6B as 4th FIFO data set + * according to this schema: + * B0 = ts[15:8], B1 = ts[23:16], B3 = ts[7:0] + */ + ts = data[1] << 16 | data[0] << 8 | data[3]; + /* + * check if hw timestamp engine is going to + * reset (the sensor generates an interrupt + * to signal the hw timestamp will reset in + * 1.638s) + */ + if (!reset_ts && ts >= 0xff0000) + reset_ts = true; + ts *= ST_LSM6DSX_TS_SENSITIVITY; + + offset += ST_LSM6DSX_SAMPLE_SIZE; + } + + if (gyro_sip-- > 0) + iio_push_to_buffers_with_timestamp( + hw->iio_devs[ST_LSM6DSX_ID_GYRO], + gyro_buff, gyro_sensor->ts_ref + ts); + if (acc_sip-- > 0) + iio_push_to_buffers_with_timestamp( + hw->iio_devs[ST_LSM6DSX_ID_ACC], + acc_buff, acc_sensor->ts_ref + ts); + } + } + + if (unlikely(reset_ts)) { + err = st_lsm6dsx_reset_hw_ts(hw); + if (err < 0) { + dev_err(hw->dev, "failed to reset hw ts (err=%d)\n", + err); + return err; + } + } + return read_len; +} + +int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw) +{ + int err; + + mutex_lock(&hw->fifo_lock); + + st_lsm6dsx_read_fifo(hw); + err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS); + + mutex_unlock(&hw->fifo_lock); + + return err; +} + +static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsx_hw *hw = sensor->hw; + int err; + + mutex_lock(&hw->conf_lock); + + if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) { + err = st_lsm6dsx_flush_fifo(hw); + if (err < 0) + goto out; + } + + if (enable) { + err = st_lsm6dsx_sensor_enable(sensor); + if (err < 0) + goto out; + } else { + err = st_lsm6dsx_sensor_disable(sensor); + if (err < 0) + goto out; + } + + err = st_lsm6dsx_set_fifo_odr(sensor, enable); + if (err < 0) + goto out; + + err = st_lsm6dsx_update_decimators(hw); + if (err < 0) + goto out; + + err = st_lsm6dsx_update_watermark(sensor, sensor->watermark); + if (err < 0) + goto out; + + if (hw->enable_mask) { + /* reset hw ts counter */ + err = st_lsm6dsx_reset_hw_ts(hw); + if (err < 0) + goto out; + + err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT); + } + +out: + mutex_unlock(&hw->conf_lock); + + return err; +} + +static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private) +{ + struct st_lsm6dsx_hw *hw = private; + + return hw->sip > 0 ? IRQ_WAKE_THREAD : IRQ_NONE; +} + +static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private) +{ + struct st_lsm6dsx_hw *hw = private; + int fifo_len = 0, len; + + /* + * If we are using edge IRQs, new samples can arrive while + * processing current interrupt since there are no hw + * guarantees the irq line stays "low" long enough to properly + * detect the new interrupt. In this case the new sample will + * be missed. + * Polling FIFO status register allow us to read new + * samples even if the interrupt arrives while processing + * previous data and the timeslot where the line is "low" is + * too short to be properly detected. + */ + do { + mutex_lock(&hw->fifo_lock); + len = st_lsm6dsx_read_fifo(hw); + mutex_unlock(&hw->fifo_lock); + + if (len > 0) + fifo_len += len; + } while (len > 0); + + return fifo_len ? IRQ_HANDLED : IRQ_NONE; +} + +static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev) +{ + return st_lsm6dsx_update_fifo(iio_dev, true); +} + +static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev) +{ + return st_lsm6dsx_update_fifo(iio_dev, false); +} + +static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = { + .preenable = st_lsm6dsx_buffer_preenable, + .postdisable = st_lsm6dsx_buffer_postdisable, +}; + +int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw) +{ + struct device_node *np = hw->dev->of_node; + struct st_sensors_platform_data *pdata; + struct iio_buffer *buffer; + unsigned long irq_type; + bool irq_active_low; + int i, err; + + irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq)); + + switch (irq_type) { + case IRQF_TRIGGER_HIGH: + case IRQF_TRIGGER_RISING: + irq_active_low = false; + break; + case IRQF_TRIGGER_LOW: + case IRQF_TRIGGER_FALLING: + irq_active_low = true; + break; + default: + dev_info(hw->dev, "mode %lx unsupported\n", irq_type); + return -EINVAL; + } + + err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_HLACTIVE_ADDR, + ST_LSM6DSX_REG_HLACTIVE_MASK, + FIELD_PREP(ST_LSM6DSX_REG_HLACTIVE_MASK, + irq_active_low)); + if (err < 0) + return err; + + pdata = (struct st_sensors_platform_data *)hw->dev->platform_data; + if ((np && of_property_read_bool(np, "drive-open-drain")) || + (pdata && pdata->open_drain)) { + err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_PP_OD_ADDR, + ST_LSM6DSX_REG_PP_OD_MASK, + FIELD_PREP(ST_LSM6DSX_REG_PP_OD_MASK, + 1)); + if (err < 0) + return err; + + irq_type |= IRQF_SHARED; + } + + err = devm_request_threaded_irq(hw->dev, hw->irq, + st_lsm6dsx_handler_irq, + st_lsm6dsx_handler_thread, + irq_type | IRQF_ONESHOT, + "lsm6dsx", hw); + if (err) { + dev_err(hw->dev, "failed to request trigger irq %d\n", + hw->irq); + return err; + } + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + buffer = devm_iio_kfifo_allocate(hw->dev); + if (!buffer) + return -ENOMEM; + + iio_device_attach_buffer(hw->iio_devs[i], buffer); + hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE; + hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops; + } + + return 0; +} diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c new file mode 100644 index 000000000..aebbe0ddd --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c @@ -0,0 +1,915 @@ +/* + * STMicroelectronics st_lsm6dsx sensor driver + * + * The ST LSM6DSx IMU MEMS series consists of 3D digital accelerometer + * and 3D digital gyroscope system-in-package with a digital I2C/SPI serial + * interface standard output. + * LSM6DSx IMU MEMS series has a dynamic user-selectable full-scale + * acceleration range of +-2/+-4/+-8/+-16 g and an angular rate range of + * +-125/+-245/+-500/+-1000/+-2000 dps + * LSM6DSx series has an integrated First-In-First-Out (FIFO) buffer + * allowing dynamic batching of sensor data. + * + * Supported sensors: + * - LSM6DS3: + * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416 + * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16 + * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000 + * - FIFO size: 8KB + * + * - LSM6DS3H/LSM6DSL/LSM6DSM/ISM330DLC: + * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416 + * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16 + * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000 + * - FIFO size: 4KB + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi <lorenzo.bianconi@st.com> + * Denis Ciocca <denis.ciocca@st.com> + * + * Licensed under the GPL-2. + */ + +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/delay.h> +#include <linux/iio/iio.h> +#include <linux/iio/sysfs.h> +#include <linux/pm.h> +#include <linux/regmap.h> +#include <linux/bitfield.h> + +#include <linux/platform_data/st_sensors_pdata.h> + +#include "st_lsm6dsx.h" + +#define ST_LSM6DSX_REG_INT1_ADDR 0x0d +#define ST_LSM6DSX_REG_INT2_ADDR 0x0e +#define ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK BIT(3) +#define ST_LSM6DSX_REG_WHOAMI_ADDR 0x0f +#define ST_LSM6DSX_REG_RESET_ADDR 0x12 +#define ST_LSM6DSX_REG_RESET_MASK BIT(0) +#define ST_LSM6DSX_REG_BDU_ADDR 0x12 +#define ST_LSM6DSX_REG_BDU_MASK BIT(6) +#define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR 0x13 +#define ST_LSM6DSX_REG_INT2_ON_INT1_MASK BIT(5) + +#define ST_LSM6DSX_REG_ACC_ODR_ADDR 0x10 +#define ST_LSM6DSX_REG_ACC_ODR_MASK GENMASK(7, 4) +#define ST_LSM6DSX_REG_ACC_FS_ADDR 0x10 +#define ST_LSM6DSX_REG_ACC_FS_MASK GENMASK(3, 2) +#define ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR 0x28 +#define ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR 0x2a +#define ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR 0x2c + +#define ST_LSM6DSX_REG_GYRO_ODR_ADDR 0x11 +#define ST_LSM6DSX_REG_GYRO_ODR_MASK GENMASK(7, 4) +#define ST_LSM6DSX_REG_GYRO_FS_ADDR 0x11 +#define ST_LSM6DSX_REG_GYRO_FS_MASK GENMASK(3, 2) +#define ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR 0x22 +#define ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR 0x24 +#define ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR 0x26 + +#define ST_LSM6DSX_ACC_FS_2G_GAIN IIO_G_TO_M_S_2(61) +#define ST_LSM6DSX_ACC_FS_4G_GAIN IIO_G_TO_M_S_2(122) +#define ST_LSM6DSX_ACC_FS_8G_GAIN IIO_G_TO_M_S_2(244) +#define ST_LSM6DSX_ACC_FS_16G_GAIN IIO_G_TO_M_S_2(488) + +#define ST_LSM6DSX_GYRO_FS_245_GAIN IIO_DEGREE_TO_RAD(8750) +#define ST_LSM6DSX_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(17500) +#define ST_LSM6DSX_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(35000) +#define ST_LSM6DSX_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000) + +struct st_lsm6dsx_odr { + u16 hz; + u8 val; +}; + +#define ST_LSM6DSX_ODR_LIST_SIZE 6 +struct st_lsm6dsx_odr_table_entry { + struct st_lsm6dsx_reg reg; + struct st_lsm6dsx_odr odr_avl[ST_LSM6DSX_ODR_LIST_SIZE]; +}; + +static const struct st_lsm6dsx_odr_table_entry st_lsm6dsx_odr_table[] = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = ST_LSM6DSX_REG_ACC_ODR_ADDR, + .mask = ST_LSM6DSX_REG_ACC_ODR_MASK, + }, + .odr_avl[0] = { 13, 0x01 }, + .odr_avl[1] = { 26, 0x02 }, + .odr_avl[2] = { 52, 0x03 }, + .odr_avl[3] = { 104, 0x04 }, + .odr_avl[4] = { 208, 0x05 }, + .odr_avl[5] = { 416, 0x06 }, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = ST_LSM6DSX_REG_GYRO_ODR_ADDR, + .mask = ST_LSM6DSX_REG_GYRO_ODR_MASK, + }, + .odr_avl[0] = { 13, 0x01 }, + .odr_avl[1] = { 26, 0x02 }, + .odr_avl[2] = { 52, 0x03 }, + .odr_avl[3] = { 104, 0x04 }, + .odr_avl[4] = { 208, 0x05 }, + .odr_avl[5] = { 416, 0x06 }, + } +}; + +struct st_lsm6dsx_fs { + u32 gain; + u8 val; +}; + +#define ST_LSM6DSX_FS_LIST_SIZE 4 +struct st_lsm6dsx_fs_table_entry { + struct st_lsm6dsx_reg reg; + struct st_lsm6dsx_fs fs_avl[ST_LSM6DSX_FS_LIST_SIZE]; +}; + +static const struct st_lsm6dsx_fs_table_entry st_lsm6dsx_fs_table[] = { + [ST_LSM6DSX_ID_ACC] = { + .reg = { + .addr = ST_LSM6DSX_REG_ACC_FS_ADDR, + .mask = ST_LSM6DSX_REG_ACC_FS_MASK, + }, + .fs_avl[0] = { ST_LSM6DSX_ACC_FS_2G_GAIN, 0x0 }, + .fs_avl[1] = { ST_LSM6DSX_ACC_FS_4G_GAIN, 0x2 }, + .fs_avl[2] = { ST_LSM6DSX_ACC_FS_8G_GAIN, 0x3 }, + .fs_avl[3] = { ST_LSM6DSX_ACC_FS_16G_GAIN, 0x1 }, + }, + [ST_LSM6DSX_ID_GYRO] = { + .reg = { + .addr = ST_LSM6DSX_REG_GYRO_FS_ADDR, + .mask = ST_LSM6DSX_REG_GYRO_FS_MASK, + }, + .fs_avl[0] = { ST_LSM6DSX_GYRO_FS_245_GAIN, 0x0 }, + .fs_avl[1] = { ST_LSM6DSX_GYRO_FS_500_GAIN, 0x1 }, + .fs_avl[2] = { ST_LSM6DSX_GYRO_FS_1000_GAIN, 0x2 }, + .fs_avl[3] = { ST_LSM6DSX_GYRO_FS_2000_GAIN, 0x3 }, + } +}; + +static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { + { + .wai = 0x69, + .max_fifo_size = 1365, + .id = { + [0] = ST_LSM6DS3_ID, + }, + .decimator = { + [ST_LSM6DSX_ID_ACC] = { + .addr = 0x08, + .mask = GENMASK(2, 0), + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = 0x08, + .mask = GENMASK(5, 3), + }, + }, + .fifo_ops = { + .fifo_th = { + .addr = 0x06, + .mask = GENMASK(11, 0), + }, + .fifo_diff = { + .addr = 0x3a, + .mask = GENMASK(11, 0), + }, + .th_wl = 3, /* 1LSB = 2B */ + }, + .ts_settings = { + .timer_en = { + .addr = 0x58, + .mask = BIT(7), + }, + .hr_timer = { + .addr = 0x5c, + .mask = BIT(4), + }, + .fifo_en = { + .addr = 0x07, + .mask = BIT(7), + }, + .decimator = { + .addr = 0x09, + .mask = GENMASK(5, 3), + }, + }, + }, + { + .wai = 0x69, + .max_fifo_size = 682, + .id = { + [0] = ST_LSM6DS3H_ID, + }, + .decimator = { + [ST_LSM6DSX_ID_ACC] = { + .addr = 0x08, + .mask = GENMASK(2, 0), + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = 0x08, + .mask = GENMASK(5, 3), + }, + }, + .fifo_ops = { + .fifo_th = { + .addr = 0x06, + .mask = GENMASK(11, 0), + }, + .fifo_diff = { + .addr = 0x3a, + .mask = GENMASK(11, 0), + }, + .th_wl = 3, /* 1LSB = 2B */ + }, + .ts_settings = { + .timer_en = { + .addr = 0x58, + .mask = BIT(7), + }, + .hr_timer = { + .addr = 0x5c, + .mask = BIT(4), + }, + .fifo_en = { + .addr = 0x07, + .mask = BIT(7), + }, + .decimator = { + .addr = 0x09, + .mask = GENMASK(5, 3), + }, + }, + }, + { + .wai = 0x6a, + .max_fifo_size = 682, + .id = { + [0] = ST_LSM6DSL_ID, + [1] = ST_LSM6DSM_ID, + [2] = ST_ISM330DLC_ID, + }, + .decimator = { + [ST_LSM6DSX_ID_ACC] = { + .addr = 0x08, + .mask = GENMASK(2, 0), + }, + [ST_LSM6DSX_ID_GYRO] = { + .addr = 0x08, + .mask = GENMASK(5, 3), + }, + }, + .fifo_ops = { + .fifo_th = { + .addr = 0x06, + .mask = GENMASK(10, 0), + }, + .fifo_diff = { + .addr = 0x3a, + .mask = GENMASK(10, 0), + }, + .th_wl = 3, /* 1LSB = 2B */ + }, + .ts_settings = { + .timer_en = { + .addr = 0x19, + .mask = BIT(5), + }, + .hr_timer = { + .addr = 0x5c, + .mask = BIT(4), + }, + .fifo_en = { + .addr = 0x07, + .mask = BIT(7), + }, + .decimator = { + .addr = 0x09, + .mask = GENMASK(5, 3), + }, + }, + }, +}; + +#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \ +{ \ + .type = chan_type, \ + .address = addr, \ + .modified = 1, \ + .channel2 = mod, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = scan_idx, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ +} + +static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = { + ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR, + IIO_MOD_X, 0), + ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR, + IIO_MOD_Y, 1), + ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR, + IIO_MOD_Z, 2), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = { + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR, + IIO_MOD_X, 0), + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR, + IIO_MOD_Y, 1), + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR, + IIO_MOD_Z, 2), + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id) +{ + int err, i, j, data; + + for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) { + for (j = 0; j < ST_LSM6DSX_MAX_ID; j++) { + if (id == st_lsm6dsx_sensor_settings[i].id[j]) + break; + } + if (j < ST_LSM6DSX_MAX_ID) + break; + } + + if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) { + dev_err(hw->dev, "unsupported hw id [%02x]\n", id); + return -ENODEV; + } + + err = regmap_read(hw->regmap, ST_LSM6DSX_REG_WHOAMI_ADDR, &data); + if (err < 0) { + dev_err(hw->dev, "failed to read whoami register\n"); + return err; + } + + if (data != st_lsm6dsx_sensor_settings[i].wai) { + dev_err(hw->dev, "unsupported whoami [%02x]\n", data); + return -ENODEV; + } + + hw->settings = &st_lsm6dsx_sensor_settings[i]; + + return 0; +} + +static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor, + u32 gain) +{ + struct st_lsm6dsx_hw *hw = sensor->hw; + const struct st_lsm6dsx_reg *reg; + int i, err; + u8 val; + + for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++) + if (st_lsm6dsx_fs_table[sensor->id].fs_avl[i].gain == gain) + break; + + if (i == ST_LSM6DSX_FS_LIST_SIZE) + return -EINVAL; + + val = st_lsm6dsx_fs_table[sensor->id].fs_avl[i].val; + reg = &st_lsm6dsx_fs_table[sensor->id].reg; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(val, reg->mask)); + if (err < 0) + return err; + + sensor->gain = gain; + + return 0; +} + +static int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr, + u8 *val) +{ + int i; + + for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) + if (st_lsm6dsx_odr_table[sensor->id].odr_avl[i].hz == odr) + break; + + if (i == ST_LSM6DSX_ODR_LIST_SIZE) + return -EINVAL; + + *val = st_lsm6dsx_odr_table[sensor->id].odr_avl[i].val; + + return 0; +} + +static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr) +{ + struct st_lsm6dsx_hw *hw = sensor->hw; + const struct st_lsm6dsx_reg *reg; + int err; + u8 val; + + err = st_lsm6dsx_check_odr(sensor, odr, &val); + if (err < 0) + return err; + + reg = &st_lsm6dsx_odr_table[sensor->id].reg; + return regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(val, reg->mask)); +} + +int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor) +{ + int err; + + err = st_lsm6dsx_set_odr(sensor, sensor->odr); + if (err < 0) + return err; + + sensor->hw->enable_mask |= BIT(sensor->id); + + return 0; +} + +int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor) +{ + struct st_lsm6dsx_hw *hw = sensor->hw; + const struct st_lsm6dsx_reg *reg; + int err; + + reg = &st_lsm6dsx_odr_table[sensor->id].reg; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(0, reg->mask)); + if (err < 0) + return err; + + sensor->hw->enable_mask &= ~BIT(sensor->id); + + return 0; +} + +static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor, + u8 addr, int *val) +{ + struct st_lsm6dsx_hw *hw = sensor->hw; + int err, delay; + __le16 data; + + err = st_lsm6dsx_sensor_enable(sensor); + if (err < 0) + return err; + + delay = 1000000 / sensor->odr; + usleep_range(delay, 2 * delay); + + err = regmap_bulk_read(hw->regmap, addr, &data, sizeof(data)); + if (err < 0) + return err; + + st_lsm6dsx_sensor_disable(sensor); + + *val = (s16)le16_to_cpu(data); + + return IIO_VAL_INT; +} + +static int st_lsm6dsx_read_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, + int *val, int *val2, long mask) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = iio_device_claim_direct_mode(iio_dev); + if (ret) + break; + + ret = st_lsm6dsx_read_oneshot(sensor, ch->address, val); + iio_device_release_direct_mode(iio_dev); + break; + case IIO_CHAN_INFO_SAMP_FREQ: + *val = sensor->odr; + ret = IIO_VAL_INT; + break; + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = sensor->gain; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + int err; + + err = iio_device_claim_direct_mode(iio_dev); + if (err) + return err; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + err = st_lsm6dsx_set_full_scale(sensor, val2); + break; + case IIO_CHAN_INFO_SAMP_FREQ: { + u8 data; + + err = st_lsm6dsx_check_odr(sensor, val, &data); + if (!err) + sensor->odr = val; + break; + } + default: + err = -EINVAL; + break; + } + + iio_device_release_direct_mode(iio_dev); + + return err; +} + +static int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + struct st_lsm6dsx_hw *hw = sensor->hw; + int err; + + if (val < 1 || val > hw->settings->max_fifo_size) + return -EINVAL; + + mutex_lock(&hw->conf_lock); + + err = st_lsm6dsx_update_watermark(sensor, val); + + mutex_unlock(&hw->conf_lock); + + if (err < 0) + return err; + + sensor->watermark = val; + + return 0; +} + +static ssize_t +st_lsm6dsx_sysfs_sampling_frequency_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsx_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", + st_lsm6dsx_odr_table[id].odr_avl[i].hz); + buf[len - 1] = '\n'; + + return len; +} + +static ssize_t st_lsm6dsx_sysfs_scale_avail(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev)); + enum st_lsm6dsx_sensor_id id = sensor->id; + int i, len = 0; + + for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++) + len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", + st_lsm6dsx_fs_table[id].fs_avl[i].gain); + buf[len - 1] = '\n'; + + return len; +} + +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_sysfs_sampling_frequency_avail); +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444, + st_lsm6dsx_sysfs_scale_avail, NULL, 0); +static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444, + st_lsm6dsx_sysfs_scale_avail, NULL, 0); + +static struct attribute *st_lsm6dsx_acc_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_accel_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsx_acc_attribute_group = { + .attrs = st_lsm6dsx_acc_attributes, +}; + +static const struct iio_info st_lsm6dsx_acc_info = { + .attrs = &st_lsm6dsx_acc_attribute_group, + .read_raw = st_lsm6dsx_read_raw, + .write_raw = st_lsm6dsx_write_raw, + .hwfifo_set_watermark = st_lsm6dsx_set_watermark, +}; + +static struct attribute *st_lsm6dsx_gyro_attributes[] = { + &iio_dev_attr_sampling_frequency_available.dev_attr.attr, + &iio_dev_attr_in_anglvel_scale_available.dev_attr.attr, + NULL, +}; + +static const struct attribute_group st_lsm6dsx_gyro_attribute_group = { + .attrs = st_lsm6dsx_gyro_attributes, +}; + +static const struct iio_info st_lsm6dsx_gyro_info = { + .attrs = &st_lsm6dsx_gyro_attribute_group, + .read_raw = st_lsm6dsx_read_raw, + .write_raw = st_lsm6dsx_write_raw, + .hwfifo_set_watermark = st_lsm6dsx_set_watermark, +}; + +static const unsigned long st_lsm6dsx_available_scan_masks[] = {0x7, 0x0}; + +static int st_lsm6dsx_of_get_drdy_pin(struct st_lsm6dsx_hw *hw, int *drdy_pin) +{ + struct device_node *np = hw->dev->of_node; + + if (!np) + return -EINVAL; + + return of_property_read_u32(np, "st,drdy-int-pin", drdy_pin); +} + +static int st_lsm6dsx_get_drdy_reg(struct st_lsm6dsx_hw *hw, u8 *drdy_reg) +{ + int err = 0, drdy_pin; + + if (st_lsm6dsx_of_get_drdy_pin(hw, &drdy_pin) < 0) { + struct st_sensors_platform_data *pdata; + struct device *dev = hw->dev; + + pdata = (struct st_sensors_platform_data *)dev->platform_data; + drdy_pin = pdata ? pdata->drdy_int_pin : 1; + } + + switch (drdy_pin) { + case 1: + *drdy_reg = ST_LSM6DSX_REG_INT1_ADDR; + break; + case 2: + *drdy_reg = ST_LSM6DSX_REG_INT2_ADDR; + break; + default: + dev_err(hw->dev, "unsupported data ready pin\n"); + err = -EINVAL; + break; + } + + return err; +} + +static int st_lsm6dsx_init_hw_timer(struct st_lsm6dsx_hw *hw) +{ + const struct st_lsm6dsx_hw_ts_settings *ts_settings; + int err, val; + + ts_settings = &hw->settings->ts_settings; + /* enable hw timestamp generation if necessary */ + if (ts_settings->timer_en.addr) { + val = ST_LSM6DSX_SHIFT_VAL(1, ts_settings->timer_en.mask); + err = regmap_update_bits(hw->regmap, + ts_settings->timer_en.addr, + ts_settings->timer_en.mask, val); + if (err < 0) + return err; + } + + /* enable high resolution for hw ts timer if necessary */ + if (ts_settings->hr_timer.addr) { + val = ST_LSM6DSX_SHIFT_VAL(1, ts_settings->hr_timer.mask); + err = regmap_update_bits(hw->regmap, + ts_settings->hr_timer.addr, + ts_settings->hr_timer.mask, val); + if (err < 0) + return err; + } + + /* enable ts queueing in FIFO if necessary */ + if (ts_settings->fifo_en.addr) { + val = ST_LSM6DSX_SHIFT_VAL(1, ts_settings->fifo_en.mask); + err = regmap_update_bits(hw->regmap, + ts_settings->fifo_en.addr, + ts_settings->fifo_en.mask, val); + if (err < 0) + return err; + } + return 0; +} + +static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw) +{ + u8 drdy_int_reg; + int err; + + err = regmap_write(hw->regmap, ST_LSM6DSX_REG_RESET_ADDR, + ST_LSM6DSX_REG_RESET_MASK); + if (err < 0) + return err; + + msleep(200); + + /* enable Block Data Update */ + err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_BDU_ADDR, + ST_LSM6DSX_REG_BDU_MASK, + FIELD_PREP(ST_LSM6DSX_REG_BDU_MASK, 1)); + if (err < 0) + return err; + + /* enable FIFO watermak interrupt */ + err = st_lsm6dsx_get_drdy_reg(hw, &drdy_int_reg); + if (err < 0) + return err; + + err = regmap_update_bits(hw->regmap, drdy_int_reg, + ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK, + FIELD_PREP(ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK, + 1)); + if (err < 0) + return err; + + return st_lsm6dsx_init_hw_timer(hw); +} + +static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw, + enum st_lsm6dsx_sensor_id id, + const char *name) +{ + struct st_lsm6dsx_sensor *sensor; + struct iio_dev *iio_dev; + + iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor)); + if (!iio_dev) + return NULL; + + iio_dev->modes = INDIO_DIRECT_MODE; + iio_dev->dev.parent = hw->dev; + iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks; + + sensor = iio_priv(iio_dev); + sensor->id = id; + sensor->hw = hw; + sensor->odr = st_lsm6dsx_odr_table[id].odr_avl[0].hz; + sensor->gain = st_lsm6dsx_fs_table[id].fs_avl[0].gain; + sensor->watermark = 1; + + switch (id) { + case ST_LSM6DSX_ID_ACC: + iio_dev->channels = st_lsm6dsx_acc_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_acc_channels); + iio_dev->info = &st_lsm6dsx_acc_info; + + scnprintf(sensor->name, sizeof(sensor->name), "%s_accel", + name); + break; + case ST_LSM6DSX_ID_GYRO: + iio_dev->channels = st_lsm6dsx_gyro_channels; + iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_gyro_channels); + iio_dev->info = &st_lsm6dsx_gyro_info; + + scnprintf(sensor->name, sizeof(sensor->name), "%s_gyro", + name); + break; + default: + return NULL; + } + iio_dev->name = sensor->name; + + return iio_dev; +} + +int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name, + struct regmap *regmap) +{ + struct st_lsm6dsx_hw *hw; + int i, err; + + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL); + if (!hw) + return -ENOMEM; + + dev_set_drvdata(dev, (void *)hw); + + mutex_init(&hw->fifo_lock); + mutex_init(&hw->conf_lock); + + hw->buff = devm_kzalloc(dev, ST_LSM6DSX_BUFF_SIZE, GFP_KERNEL); + if (!hw->buff) + return -ENOMEM; + + hw->dev = dev; + hw->irq = irq; + hw->regmap = regmap; + + err = st_lsm6dsx_check_whoami(hw, hw_id); + if (err < 0) + return err; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + hw->iio_devs[i] = st_lsm6dsx_alloc_iiodev(hw, i, name); + if (!hw->iio_devs[i]) + return -ENOMEM; + } + + err = st_lsm6dsx_init_device(hw); + if (err < 0) + return err; + + if (hw->irq > 0) { + err = st_lsm6dsx_fifo_setup(hw); + if (err < 0) + return err; + } + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + err = devm_iio_device_register(hw->dev, hw->iio_devs[i]); + if (err) + return err; + } + + return 0; +} +EXPORT_SYMBOL(st_lsm6dsx_probe); + +static int __maybe_unused st_lsm6dsx_suspend(struct device *dev) +{ + struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dsx_sensor *sensor; + const struct st_lsm6dsx_reg *reg; + int i, err = 0; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + reg = &st_lsm6dsx_odr_table[sensor->id].reg; + err = regmap_update_bits(hw->regmap, reg->addr, reg->mask, + ST_LSM6DSX_SHIFT_VAL(0, reg->mask)); + if (err < 0) + return err; + } + + if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) + err = st_lsm6dsx_flush_fifo(hw); + + return err; +} + +static int __maybe_unused st_lsm6dsx_resume(struct device *dev) +{ + struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev); + struct st_lsm6dsx_sensor *sensor; + int i, err = 0; + + for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) { + sensor = iio_priv(hw->iio_devs[i]); + if (!(hw->enable_mask & BIT(sensor->id))) + continue; + + err = st_lsm6dsx_set_odr(sensor, sensor->odr); + if (err < 0) + return err; + } + + if (hw->enable_mask) + err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT); + + return err; +} + +const struct dev_pm_ops st_lsm6dsx_pm_ops = { + SET_SYSTEM_SLEEP_PM_OPS(st_lsm6dsx_suspend, st_lsm6dsx_resume) +}; +EXPORT_SYMBOL(st_lsm6dsx_pm_ops); + +MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>"); +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c new file mode 100644 index 000000000..377c4e999 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c @@ -0,0 +1,92 @@ +/* + * STMicroelectronics st_lsm6dsx i2c driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi <lorenzo.bianconi@st.com> + * Denis Ciocca <denis.ciocca@st.com> + * + * Licensed under the GPL-2. + */ + +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/i2c.h> +#include <linux/slab.h> +#include <linux/of.h> +#include <linux/regmap.h> + +#include "st_lsm6dsx.h" + +static const struct regmap_config st_lsm6dsx_i2c_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dsx_i2c_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + int hw_id = id->driver_data; + struct regmap *regmap; + + regmap = devm_regmap_init_i2c(client, &st_lsm6dsx_i2c_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&client->dev, "Failed to register i2c regmap %d\n", + (int)PTR_ERR(regmap)); + return PTR_ERR(regmap); + } + + return st_lsm6dsx_probe(&client->dev, client->irq, + hw_id, id->name, regmap); +} + +static const struct of_device_id st_lsm6dsx_i2c_of_match[] = { + { + .compatible = "st,lsm6ds3", + .data = (void *)ST_LSM6DS3_ID, + }, + { + .compatible = "st,lsm6ds3h", + .data = (void *)ST_LSM6DS3H_ID, + }, + { + .compatible = "st,lsm6dsl", + .data = (void *)ST_LSM6DSL_ID, + }, + { + .compatible = "st,lsm6dsm", + .data = (void *)ST_LSM6DSM_ID, + }, + { + .compatible = "st,ism330dlc", + .data = (void *)ST_ISM330DLC_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsx_i2c_of_match); + +static const struct i2c_device_id st_lsm6dsx_i2c_id_table[] = { + { ST_LSM6DS3_DEV_NAME, ST_LSM6DS3_ID }, + { ST_LSM6DS3H_DEV_NAME, ST_LSM6DS3H_ID }, + { ST_LSM6DSL_DEV_NAME, ST_LSM6DSL_ID }, + { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID }, + { ST_ISM330DLC_DEV_NAME, ST_ISM330DLC_ID }, + {}, +}; +MODULE_DEVICE_TABLE(i2c, st_lsm6dsx_i2c_id_table); + +static struct i2c_driver st_lsm6dsx_driver = { + .driver = { + .name = "st_lsm6dsx_i2c", + .pm = &st_lsm6dsx_pm_ops, + .of_match_table = of_match_ptr(st_lsm6dsx_i2c_of_match), + }, + .probe = st_lsm6dsx_i2c_probe, + .id_table = st_lsm6dsx_i2c_id_table, +}; +module_i2c_driver(st_lsm6dsx_driver); + +MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>"); +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx i2c driver"); +MODULE_LICENSE("GPL v2"); diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c new file mode 100644 index 000000000..fec5c6ce7 --- /dev/null +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c @@ -0,0 +1,92 @@ +/* + * STMicroelectronics st_lsm6dsx spi driver + * + * Copyright 2016 STMicroelectronics Inc. + * + * Lorenzo Bianconi <lorenzo.bianconi@st.com> + * Denis Ciocca <denis.ciocca@st.com> + * + * Licensed under the GPL-2. + */ + +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/spi/spi.h> +#include <linux/slab.h> +#include <linux/of.h> +#include <linux/regmap.h> + +#include "st_lsm6dsx.h" + +static const struct regmap_config st_lsm6dsx_spi_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int st_lsm6dsx_spi_probe(struct spi_device *spi) +{ + const struct spi_device_id *id = spi_get_device_id(spi); + int hw_id = id->driver_data; + struct regmap *regmap; + + regmap = devm_regmap_init_spi(spi, &st_lsm6dsx_spi_regmap_config); + if (IS_ERR(regmap)) { + dev_err(&spi->dev, "Failed to register spi regmap %d\n", + (int)PTR_ERR(regmap)); + return PTR_ERR(regmap); + } + + return st_lsm6dsx_probe(&spi->dev, spi->irq, + hw_id, id->name, regmap); +} + +static const struct of_device_id st_lsm6dsx_spi_of_match[] = { + { + .compatible = "st,lsm6ds3", + .data = (void *)ST_LSM6DS3_ID, + }, + { + .compatible = "st,lsm6ds3h", + .data = (void *)ST_LSM6DS3H_ID, + }, + { + .compatible = "st,lsm6dsl", + .data = (void *)ST_LSM6DSL_ID, + }, + { + .compatible = "st,lsm6dsm", + .data = (void *)ST_LSM6DSM_ID, + }, + { + .compatible = "st,ism330dlc", + .data = (void *)ST_ISM330DLC_ID, + }, + {}, +}; +MODULE_DEVICE_TABLE(of, st_lsm6dsx_spi_of_match); + +static const struct spi_device_id st_lsm6dsx_spi_id_table[] = { + { ST_LSM6DS3_DEV_NAME, ST_LSM6DS3_ID }, + { ST_LSM6DS3H_DEV_NAME, ST_LSM6DS3H_ID }, + { ST_LSM6DSL_DEV_NAME, ST_LSM6DSL_ID }, + { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID }, + { ST_ISM330DLC_DEV_NAME, ST_ISM330DLC_ID }, + {}, +}; +MODULE_DEVICE_TABLE(spi, st_lsm6dsx_spi_id_table); + +static struct spi_driver st_lsm6dsx_driver = { + .driver = { + .name = "st_lsm6dsx_spi", + .pm = &st_lsm6dsx_pm_ops, + .of_match_table = of_match_ptr(st_lsm6dsx_spi_of_match), + }, + .probe = st_lsm6dsx_spi_probe, + .id_table = st_lsm6dsx_spi_id_table, +}; +module_spi_driver(st_lsm6dsx_driver); + +MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>"); +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx spi driver"); +MODULE_LICENSE("GPL v2"); |