mirror of
https://github.com/Fishwaldo/Star64_linux.git
synced 2025-04-19 12:54:00 +00:00
iio: imu: add support to lsm6dsx driver
Add support to STM LSM6DS3-LSM6DSM 6-axis (acc + gyro) Mems sensor http://www.st.com/resource/en/datasheet/lsm6ds3.pdf http://www.st.com/resource/en/datasheet/lsm6dsm.pdf - continuous mode support - i2c support - spi support - sw fifo mode support - supported devices: lsm6ds3, lsm6dsm Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi@st.com> Signed-off-by: Jonathan Cameron <jic23@kernel.org>
This commit is contained in:
Lorenzo Bianconi
Jonathan Cameron
parent
adc8ec5ff1
commit
290a6ce11d
9 changed files with 1517 additions and 0 deletions
|
|
@ -39,6 +39,7 @@ config KMX61
|
||||||
be called kmx61.
|
be called kmx61.
|
||||||
|
|
||||||
source "drivers/iio/imu/inv_mpu6050/Kconfig"
|
source "drivers/iio/imu/inv_mpu6050/Kconfig"
|
||||||
|
source "drivers/iio/imu/st_lsm6dsx/Kconfig"
|
||||||
|
|
||||||
endmenu
|
endmenu
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -17,3 +17,5 @@ obj-y += bmi160/
|
||||||
obj-y += inv_mpu6050/
|
obj-y += inv_mpu6050/
|
||||||
|
|
||||||
obj-$(CONFIG_KMX61) += kmx61.o
|
obj-$(CONFIG_KMX61) += kmx61.o
|
||||||
|
|
||||||
|
obj-y += st_lsm6dsx/
|
||||||
|
|
|
||||||
22
drivers/iio/imu/st_lsm6dsx/Kconfig
Normal file
22
drivers/iio/imu/st_lsm6dsx/Kconfig
Normal file
|
|
@ -0,0 +1,22 @@
|
||||||
|
|
||||||
|
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, lsm6dsm
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
config IIO_ST_LSM6DSX_SPI
|
||||||
|
tristate
|
||||||
|
depends on IIO_ST_LSM6DSX
|
||||||
5
drivers/iio/imu/st_lsm6dsx/Makefile
Normal file
5
drivers/iio/imu/st_lsm6dsx/Makefile
Normal file
|
|
@ -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
|
||||||
141
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
Normal file
141
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
Normal file
|
|
@ -0,0 +1,141 @@
|
||||||
|
/*
|
||||||
|
* 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_LSM6DSM_DEV_NAME "lsm6dsm"
|
||||||
|
|
||||||
|
enum st_lsm6dsx_hw_id {
|
||||||
|
ST_LSM6DS3_ID,
|
||||||
|
ST_LSM6DSM_ID,
|
||||||
|
};
|
||||||
|
|
||||||
|
#define ST_LSM6DSX_CHAN_SIZE 2
|
||||||
|
#define ST_LSM6DSX_SAMPLE_SIZE 6
|
||||||
|
#define ST_LSM6DSX_SAMPLE_DEPTH (ST_LSM6DSX_SAMPLE_SIZE / \
|
||||||
|
ST_LSM6DSX_CHAN_SIZE)
|
||||||
|
|
||||||
|
#if defined(CONFIG_SPI_MASTER)
|
||||||
|
#define ST_LSM6DSX_RX_MAX_LENGTH 256
|
||||||
|
#define ST_LSM6DSX_TX_MAX_LENGTH 8
|
||||||
|
|
||||||
|
struct st_lsm6dsx_transfer_buffer {
|
||||||
|
u8 rx_buf[ST_LSM6DSX_RX_MAX_LENGTH];
|
||||||
|
u8 tx_buf[ST_LSM6DSX_TX_MAX_LENGTH] ____cacheline_aligned;
|
||||||
|
};
|
||||||
|
#endif /* CONFIG_SPI_MASTER */
|
||||||
|
|
||||||
|
struct st_lsm6dsx_transfer_function {
|
||||||
|
int (*read)(struct device *dev, u8 addr, int len, u8 *data);
|
||||||
|
int (*write)(struct device *dev, u8 addr, int len, u8 *data);
|
||||||
|
};
|
||||||
|
|
||||||
|
struct st_lsm6dsx_reg {
|
||||||
|
u8 addr;
|
||||||
|
u8 mask;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct st_lsm6dsx_settings {
|
||||||
|
u8 wai;
|
||||||
|
u16 max_fifo_size;
|
||||||
|
enum st_lsm6dsx_hw_id id;
|
||||||
|
};
|
||||||
|
|
||||||
|
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
|
||||||
|
* @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.
|
||||||
|
* @decimator_mask: Sensor mask for decimation register.
|
||||||
|
* @delta_ts: Delta time between two consecutive interrupts.
|
||||||
|
* @ts: Latest timestamp from the interrupt handler.
|
||||||
|
*/
|
||||||
|
struct st_lsm6dsx_sensor {
|
||||||
|
enum st_lsm6dsx_sensor_id id;
|
||||||
|
struct st_lsm6dsx_hw *hw;
|
||||||
|
|
||||||
|
u32 gain;
|
||||||
|
u16 odr;
|
||||||
|
|
||||||
|
u16 watermark;
|
||||||
|
u8 sip;
|
||||||
|
u8 decimator;
|
||||||
|
u8 decimator_mask;
|
||||||
|
|
||||||
|
s64 delta_ts;
|
||||||
|
s64 ts;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* struct st_lsm6dsx_hw - ST IMU MEMS hw instance
|
||||||
|
* @dev: Pointer to instance of struct device (I2C or SPI).
|
||||||
|
* @irq: Device interrupt line (I2C or SPI).
|
||||||
|
* @lock: Mutex to protect read and write operations.
|
||||||
|
* @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
|
||||||
|
* @fifo_mode: FIFO operating mode supported by the device.
|
||||||
|
* @enable_mask: Enabled sensor bitmask.
|
||||||
|
* @sip: Total number of samples (acc/gyro) in a given pattern.
|
||||||
|
* @iio_devs: Pointers to acc/gyro iio_dev instances.
|
||||||
|
* @settings: Pointer to the specific sensor settings in use.
|
||||||
|
* @tf: Transfer function structure used by I/O operations.
|
||||||
|
* @tb: Transfer buffers used by SPI I/O operations.
|
||||||
|
*/
|
||||||
|
struct st_lsm6dsx_hw {
|
||||||
|
struct device *dev;
|
||||||
|
int irq;
|
||||||
|
|
||||||
|
struct mutex lock;
|
||||||
|
struct mutex fifo_lock;
|
||||||
|
|
||||||
|
enum st_lsm6dsx_fifo_mode fifo_mode;
|
||||||
|
u8 enable_mask;
|
||||||
|
u8 sip;
|
||||||
|
|
||||||
|
struct iio_dev *iio_devs[ST_LSM6DSX_ID_MAX];
|
||||||
|
|
||||||
|
const struct st_lsm6dsx_settings *settings;
|
||||||
|
|
||||||
|
const struct st_lsm6dsx_transfer_function *tf;
|
||||||
|
#if defined(CONFIG_SPI_MASTER)
|
||||||
|
struct st_lsm6dsx_transfer_buffer tb;
|
||||||
|
#endif /* CONFIG_SPI_MASTER */
|
||||||
|
};
|
||||||
|
|
||||||
|
int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id,
|
||||||
|
const struct st_lsm6dsx_transfer_function *tf_ops);
|
||||||
|
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_write_with_mask(struct st_lsm6dsx_hw *hw, u8 addr, u8 mask,
|
||||||
|
u8 val);
|
||||||
|
int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor,
|
||||||
|
u16 watermark);
|
||||||
|
|
||||||
|
#endif /* ST_LSM6DSX_H */
|
||||||
454
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
Normal file
454
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
Normal file
|
|
@ -0,0 +1,454 @@
|
||||||
|
/*
|
||||||
|
* STMicroelectronics st_lsm6dsx FIFO buffer library driver
|
||||||
|
*
|
||||||
|
* LSM6DS3/LSM6DSM: 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 "st_lsm6dsx.h"
|
||||||
|
|
||||||
|
#define ST_LSM6DSX_REG_FIFO_THL_ADDR 0x06
|
||||||
|
#define ST_LSM6DSX_REG_FIFO_THH_ADDR 0x07
|
||||||
|
#define ST_LSM6DSX_FIFO_TH_MASK GENMASK(11, 0)
|
||||||
|
#define ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR 0x08
|
||||||
|
#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_REG_FIFO_DIFFL_ADDR 0x3a
|
||||||
|
#define ST_LSM6DSX_FIFO_DIFF_MASK GENMASK(11, 0)
|
||||||
|
#define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
|
||||||
|
#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
|
||||||
|
|
||||||
|
#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
|
||||||
|
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
struct st_lsm6dsx_sensor *sensor;
|
||||||
|
u16 max_odr, min_odr, sip = 0;
|
||||||
|
int err, i;
|
||||||
|
u8 data;
|
||||||
|
|
||||||
|
st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
|
||||||
|
|
||||||
|
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
err = st_lsm6dsx_write_with_mask(hw,
|
||||||
|
ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR,
|
||||||
|
sensor->decimator_mask, data);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
sip += sensor->sip;
|
||||||
|
}
|
||||||
|
hw->sip = sip;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
|
||||||
|
enum st_lsm6dsx_fifo_mode fifo_mode)
|
||||||
|
{
|
||||||
|
u8 data;
|
||||||
|
int err;
|
||||||
|
|
||||||
|
switch (fifo_mode) {
|
||||||
|
case ST_LSM6DSX_FIFO_BYPASS:
|
||||||
|
data = fifo_mode;
|
||||||
|
break;
|
||||||
|
case ST_LSM6DSX_FIFO_CONT:
|
||||||
|
data = (ST_LSM6DSX_MAX_FIFO_ODR_VAL <<
|
||||||
|
__ffs(ST_LSM6DSX_FIFO_ODR_MASK)) | fifo_mode;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
|
err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
|
||||||
|
sizeof(data), &data);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
hw->fifo_mode = fifo_mode;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
|
||||||
|
{
|
||||||
|
u16 fifo_watermark = ~0, cur_watermark, sip = 0;
|
||||||
|
struct st_lsm6dsx_hw *hw = sensor->hw;
|
||||||
|
struct st_lsm6dsx_sensor *cur_sensor;
|
||||||
|
__le16 wdata;
|
||||||
|
int i, err;
|
||||||
|
u8 data;
|
||||||
|
|
||||||
|
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);
|
||||||
|
sip += cur_sensor->sip;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!sip)
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
fifo_watermark = max_t(u16, fifo_watermark, sip);
|
||||||
|
fifo_watermark = (fifo_watermark / sip) * sip;
|
||||||
|
fifo_watermark = fifo_watermark * ST_LSM6DSX_SAMPLE_DEPTH;
|
||||||
|
|
||||||
|
mutex_lock(&hw->lock);
|
||||||
|
|
||||||
|
err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_THH_ADDR,
|
||||||
|
sizeof(data), &data);
|
||||||
|
if (err < 0)
|
||||||
|
goto out;
|
||||||
|
|
||||||
|
fifo_watermark = ((data & ~ST_LSM6DSX_FIFO_TH_MASK) << 8) |
|
||||||
|
(fifo_watermark & ST_LSM6DSX_FIFO_TH_MASK);
|
||||||
|
|
||||||
|
wdata = cpu_to_le16(fifo_watermark);
|
||||||
|
err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_THL_ADDR,
|
||||||
|
sizeof(wdata), (u8 *)&wdata);
|
||||||
|
out:
|
||||||
|
mutex_unlock(&hw->lock);
|
||||||
|
|
||||||
|
return err < 0 ? err : 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DSM read FIFO 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;
|
||||||
|
int err, acc_sip, gyro_sip, read_len, samples, offset;
|
||||||
|
struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
|
||||||
|
s64 acc_ts, acc_delta_ts, gyro_ts, gyro_delta_ts;
|
||||||
|
u8 iio_buff[ALIGN(ST_LSM6DSX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
|
||||||
|
u8 buff[pattern_len];
|
||||||
|
__le16 fifo_status;
|
||||||
|
|
||||||
|
err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_DIFFL_ADDR,
|
||||||
|
sizeof(fifo_status), (u8 *)&fifo_status);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
fifo_len = (le16_to_cpu(fifo_status) & ST_LSM6DSX_FIFO_DIFF_MASK) *
|
||||||
|
ST_LSM6DSX_CHAN_SIZE;
|
||||||
|
samples = fifo_len / ST_LSM6DSX_SAMPLE_SIZE;
|
||||||
|
fifo_len = (fifo_len / pattern_len) * pattern_len;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* compute delta timestamp between two consecutive samples
|
||||||
|
* in order to estimate queueing time of data generated
|
||||||
|
* by the sensor
|
||||||
|
*/
|
||||||
|
acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
|
||||||
|
acc_ts = acc_sensor->ts - acc_sensor->delta_ts;
|
||||||
|
acc_delta_ts = div_s64(acc_sensor->delta_ts * acc_sensor->decimator,
|
||||||
|
samples);
|
||||||
|
|
||||||
|
gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
|
||||||
|
gyro_ts = gyro_sensor->ts - gyro_sensor->delta_ts;
|
||||||
|
gyro_delta_ts = div_s64(gyro_sensor->delta_ts * gyro_sensor->decimator,
|
||||||
|
samples);
|
||||||
|
|
||||||
|
for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
|
||||||
|
err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
|
||||||
|
sizeof(buff), buff);
|
||||||
|
if (err < 0)
|
||||||
|
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), 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, Gx, Gy, Gz
|
||||||
|
*/
|
||||||
|
gyro_sip = gyro_sensor->sip;
|
||||||
|
acc_sip = acc_sensor->sip;
|
||||||
|
offset = 0;
|
||||||
|
|
||||||
|
while (acc_sip > 0 || gyro_sip > 0) {
|
||||||
|
if (gyro_sip-- > 0) {
|
||||||
|
memcpy(iio_buff, &buff[offset],
|
||||||
|
ST_LSM6DSX_SAMPLE_SIZE);
|
||||||
|
iio_push_to_buffers_with_timestamp(
|
||||||
|
hw->iio_devs[ST_LSM6DSX_ID_GYRO],
|
||||||
|
iio_buff, gyro_ts);
|
||||||
|
offset += ST_LSM6DSX_SAMPLE_SIZE;
|
||||||
|
gyro_ts += gyro_delta_ts;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (acc_sip-- > 0) {
|
||||||
|
memcpy(iio_buff, &buff[offset],
|
||||||
|
ST_LSM6DSX_SAMPLE_SIZE);
|
||||||
|
iio_push_to_buffers_with_timestamp(
|
||||||
|
hw->iio_devs[ST_LSM6DSX_ID_ACC],
|
||||||
|
iio_buff, acc_ts);
|
||||||
|
offset += ST_LSM6DSX_SAMPLE_SIZE;
|
||||||
|
acc_ts += acc_delta_ts;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return read_len;
|
||||||
|
}
|
||||||
|
|
||||||
|
static 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;
|
||||||
|
|
||||||
|
if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
|
||||||
|
err = st_lsm6dsx_flush_fifo(hw);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (enable) {
|
||||||
|
err = st_lsm6dsx_sensor_enable(sensor);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
} else {
|
||||||
|
err = st_lsm6dsx_sensor_disable(sensor);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
}
|
||||||
|
|
||||||
|
err = st_lsm6dsx_update_decimators(hw);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
if (hw->enable_mask) {
|
||||||
|
err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* store enable buffer timestamp as reference to compute
|
||||||
|
* first delta timestamp
|
||||||
|
*/
|
||||||
|
sensor->ts = iio_get_time_ns(iio_dev);
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
|
||||||
|
{
|
||||||
|
struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
|
||||||
|
struct st_lsm6dsx_sensor *sensor;
|
||||||
|
int i;
|
||||||
|
|
||||||
|
if (!hw->sip)
|
||||||
|
return IRQ_NONE;
|
||||||
|
|
||||||
|
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
|
||||||
|
sensor = iio_priv(hw->iio_devs[i]);
|
||||||
|
|
||||||
|
if (sensor->sip > 0) {
|
||||||
|
s64 timestamp;
|
||||||
|
|
||||||
|
timestamp = iio_get_time_ns(hw->iio_devs[i]);
|
||||||
|
sensor->delta_ts = timestamp - sensor->ts;
|
||||||
|
sensor->ts = timestamp;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return IRQ_WAKE_THREAD;
|
||||||
|
}
|
||||||
|
|
||||||
|
static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
|
||||||
|
{
|
||||||
|
struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
|
||||||
|
int count;
|
||||||
|
|
||||||
|
mutex_lock(&hw->fifo_lock);
|
||||||
|
count = st_lsm6dsx_read_fifo(hw);
|
||||||
|
mutex_unlock(&hw->fifo_lock);
|
||||||
|
|
||||||
|
return !count ? IRQ_NONE : IRQ_HANDLED;
|
||||||
|
}
|
||||||
|
|
||||||
|
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 iio_buffer *buffer;
|
||||||
|
unsigned long irq_type;
|
||||||
|
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:
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
dev_info(hw->dev, "mode %lx unsupported\n", irq_type);
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
673
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
Normal file
673
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
Normal file
|
|
@ -0,0 +1,673 @@
|
||||||
|
/*
|
||||||
|
* 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
|
||||||
|
*
|
||||||
|
* - LSM6DSM:
|
||||||
|
* - 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 "st_lsm6dsx.h"
|
||||||
|
|
||||||
|
#define ST_LSM6DSX_REG_ACC_DEC_MASK GENMASK(2, 0)
|
||||||
|
#define ST_LSM6DSX_REG_GYRO_DEC_MASK GENMASK(5, 3)
|
||||||
|
#define ST_LSM6DSX_REG_INT1_ADDR 0x0d
|
||||||
|
#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_ROUNDING_ADDR 0x16
|
||||||
|
#define ST_LSM6DSX_REG_ROUNDING_MASK BIT(2)
|
||||||
|
#define ST_LSM6DSX_REG_LIR_ADDR 0x58
|
||||||
|
#define ST_LSM6DSX_REG_LIR_MASK BIT(0)
|
||||||
|
|
||||||
|
#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_LSM6DS3_WHOAMI 0x69
|
||||||
|
#define ST_LSM6DSM_WHOAMI 0x6a
|
||||||
|
|
||||||
|
#define ST_LSM6DS3_MAX_FIFO_SIZE 8192
|
||||||
|
#define ST_LSM6DSM_MAX_FIFO_SIZE 4096
|
||||||
|
|
||||||
|
#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(4375)
|
||||||
|
#define ST_LSM6DSX_GYRO_FS_500_GAIN IIO_DEGREE_TO_RAD(8750)
|
||||||
|
#define ST_LSM6DSX_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(17500)
|
||||||
|
#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 = ST_LSM6DS3_WHOAMI,
|
||||||
|
.max_fifo_size = ST_LSM6DS3_MAX_FIFO_SIZE,
|
||||||
|
.id = ST_LSM6DS3_ID,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
.wai = ST_LSM6DSM_WHOAMI,
|
||||||
|
.max_fifo_size = ST_LSM6DSM_MAX_FIFO_SIZE,
|
||||||
|
.id = ST_LSM6DSM_ID,
|
||||||
|
},
|
||||||
|
};
|
||||||
|
|
||||||
|
#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),
|
||||||
|
};
|
||||||
|
|
||||||
|
int st_lsm6dsx_write_with_mask(struct st_lsm6dsx_hw *hw, u8 addr, u8 mask,
|
||||||
|
u8 val)
|
||||||
|
{
|
||||||
|
u8 data;
|
||||||
|
int err;
|
||||||
|
|
||||||
|
mutex_lock(&hw->lock);
|
||||||
|
|
||||||
|
err = hw->tf->read(hw->dev, addr, sizeof(data), &data);
|
||||||
|
if (err < 0) {
|
||||||
|
dev_err(hw->dev, "failed to read %02x register\n", addr);
|
||||||
|
goto out;
|
||||||
|
}
|
||||||
|
|
||||||
|
data = (data & ~mask) | ((val << __ffs(mask)) & mask);
|
||||||
|
|
||||||
|
err = hw->tf->write(hw->dev, addr, sizeof(data), &data);
|
||||||
|
if (err < 0)
|
||||||
|
dev_err(hw->dev, "failed to write %02x register\n", addr);
|
||||||
|
|
||||||
|
out:
|
||||||
|
mutex_unlock(&hw->lock);
|
||||||
|
|
||||||
|
return err;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id)
|
||||||
|
{
|
||||||
|
int err, i;
|
||||||
|
u8 data;
|
||||||
|
|
||||||
|
for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
|
||||||
|
if (id == st_lsm6dsx_sensor_settings[i].id)
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) {
|
||||||
|
dev_err(hw->dev, "unsupported hw id [%02x]\n", id);
|
||||||
|
return -ENODEV;
|
||||||
|
}
|
||||||
|
|
||||||
|
err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_WHOAMI_ADDR, sizeof(data),
|
||||||
|
&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)
|
||||||
|
{
|
||||||
|
enum st_lsm6dsx_sensor_id id = sensor->id;
|
||||||
|
int i, err;
|
||||||
|
u8 val;
|
||||||
|
|
||||||
|
for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++)
|
||||||
|
if (st_lsm6dsx_fs_table[id].fs_avl[i].gain == gain)
|
||||||
|
break;
|
||||||
|
|
||||||
|
if (i == ST_LSM6DSX_FS_LIST_SIZE)
|
||||||
|
return -EINVAL;
|
||||||
|
|
||||||
|
val = st_lsm6dsx_fs_table[id].fs_avl[i].val;
|
||||||
|
err = st_lsm6dsx_write_with_mask(sensor->hw,
|
||||||
|
st_lsm6dsx_fs_table[id].reg.addr,
|
||||||
|
st_lsm6dsx_fs_table[id].reg.mask,
|
||||||
|
val);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
sensor->gain = gain;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
|
||||||
|
{
|
||||||
|
enum st_lsm6dsx_sensor_id id = sensor->id;
|
||||||
|
int i, err;
|
||||||
|
u8 val;
|
||||||
|
|
||||||
|
for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
|
||||||
|
if (st_lsm6dsx_odr_table[id].odr_avl[i].hz == odr)
|
||||||
|
break;
|
||||||
|
|
||||||
|
if (i == ST_LSM6DSX_ODR_LIST_SIZE)
|
||||||
|
return -EINVAL;
|
||||||
|
|
||||||
|
val = st_lsm6dsx_odr_table[id].odr_avl[i].val;
|
||||||
|
err = st_lsm6dsx_write_with_mask(sensor->hw,
|
||||||
|
st_lsm6dsx_odr_table[id].reg.addr,
|
||||||
|
st_lsm6dsx_odr_table[id].reg.mask,
|
||||||
|
val);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
sensor->odr = odr;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
enum st_lsm6dsx_sensor_id id = sensor->id;
|
||||||
|
int err;
|
||||||
|
|
||||||
|
err = st_lsm6dsx_write_with_mask(sensor->hw,
|
||||||
|
st_lsm6dsx_odr_table[id].reg.addr,
|
||||||
|
st_lsm6dsx_odr_table[id].reg.mask, 0);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
sensor->hw->enable_mask &= ~BIT(id);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
|
||||||
|
u8 addr, int *val)
|
||||||
|
{
|
||||||
|
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 = sensor->hw->tf->read(sensor->hw->dev, addr, sizeof(data),
|
||||||
|
(u8 *)&data);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
st_lsm6dsx_sensor_disable(sensor);
|
||||||
|
|
||||||
|
*val = (s16)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:
|
||||||
|
err = st_lsm6dsx_set_odr(sensor, 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, max_fifo_len;
|
||||||
|
|
||||||
|
max_fifo_len = hw->settings->max_fifo_size / ST_LSM6DSX_SAMPLE_SIZE;
|
||||||
|
if (val < 1 || val > max_fifo_len)
|
||||||
|
return -EINVAL;
|
||||||
|
|
||||||
|
err = st_lsm6dsx_update_watermark(sensor, val);
|
||||||
|
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 = {
|
||||||
|
.driver_module = THIS_MODULE,
|
||||||
|
.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 = {
|
||||||
|
.driver_module = THIS_MODULE,
|
||||||
|
.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_init_device(struct st_lsm6dsx_hw *hw)
|
||||||
|
{
|
||||||
|
int err;
|
||||||
|
u8 data;
|
||||||
|
|
||||||
|
data = ST_LSM6DSX_REG_RESET_MASK;
|
||||||
|
err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_RESET_ADDR, sizeof(data),
|
||||||
|
&data);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
msleep(200);
|
||||||
|
|
||||||
|
/* latch interrupts */
|
||||||
|
err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_LIR_ADDR,
|
||||||
|
ST_LSM6DSX_REG_LIR_MASK, 1);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
/* enable Block Data Update */
|
||||||
|
err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_BDU_ADDR,
|
||||||
|
ST_LSM6DSX_REG_BDU_MASK, 1);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_ROUNDING_ADDR,
|
||||||
|
ST_LSM6DSX_REG_ROUNDING_MASK, 1);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
/* enable FIFO watermak interrupt */
|
||||||
|
err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_INT1_ADDR,
|
||||||
|
ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK, 1);
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
/* redirect INT2 on INT1 */
|
||||||
|
return st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_INT2_ON_INT1_ADDR,
|
||||||
|
ST_LSM6DSX_REG_INT2_ON_INT1_MASK, 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
|
||||||
|
enum st_lsm6dsx_sensor_id id)
|
||||||
|
{
|
||||||
|
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->name = "lsm6dsx_accel";
|
||||||
|
iio_dev->info = &st_lsm6dsx_acc_info;
|
||||||
|
|
||||||
|
sensor->decimator_mask = ST_LSM6DSX_REG_ACC_DEC_MASK;
|
||||||
|
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->name = "lsm6dsx_gyro";
|
||||||
|
iio_dev->info = &st_lsm6dsx_gyro_info;
|
||||||
|
|
||||||
|
sensor->decimator_mask = ST_LSM6DSX_REG_GYRO_DEC_MASK;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
return iio_dev;
|
||||||
|
}
|
||||||
|
|
||||||
|
int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id,
|
||||||
|
const struct st_lsm6dsx_transfer_function *tf_ops)
|
||||||
|
{
|
||||||
|
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->lock);
|
||||||
|
mutex_init(&hw->fifo_lock);
|
||||||
|
|
||||||
|
hw->dev = dev;
|
||||||
|
hw->irq = irq;
|
||||||
|
hw->tf = tf_ops;
|
||||||
|
|
||||||
|
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);
|
||||||
|
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);
|
||||||
|
|
||||||
|
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");
|
||||||
101
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
Normal file
101
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
Normal file
|
|
@ -0,0 +1,101 @@
|
||||||
|
/*
|
||||||
|
* 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 "st_lsm6dsx.h"
|
||||||
|
|
||||||
|
static int st_lsm6dsx_i2c_read(struct device *dev, u8 addr, int len, u8 *data)
|
||||||
|
{
|
||||||
|
struct i2c_client *client = to_i2c_client(dev);
|
||||||
|
struct i2c_msg msg[2];
|
||||||
|
|
||||||
|
msg[0].addr = client->addr;
|
||||||
|
msg[0].flags = client->flags;
|
||||||
|
msg[0].len = 1;
|
||||||
|
msg[0].buf = &addr;
|
||||||
|
|
||||||
|
msg[1].addr = client->addr;
|
||||||
|
msg[1].flags = client->flags | I2C_M_RD;
|
||||||
|
msg[1].len = len;
|
||||||
|
msg[1].buf = data;
|
||||||
|
|
||||||
|
return i2c_transfer(client->adapter, msg, 2);
|
||||||
|
}
|
||||||
|
|
||||||
|
static int st_lsm6dsx_i2c_write(struct device *dev, u8 addr, int len, u8 *data)
|
||||||
|
{
|
||||||
|
struct i2c_client *client = to_i2c_client(dev);
|
||||||
|
struct i2c_msg msg;
|
||||||
|
u8 send[len + 1];
|
||||||
|
|
||||||
|
send[0] = addr;
|
||||||
|
memcpy(&send[1], data, len * sizeof(u8));
|
||||||
|
|
||||||
|
msg.addr = client->addr;
|
||||||
|
msg.flags = client->flags;
|
||||||
|
msg.len = len + 1;
|
||||||
|
msg.buf = send;
|
||||||
|
|
||||||
|
return i2c_transfer(client->adapter, &msg, 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
static const struct st_lsm6dsx_transfer_function st_lsm6dsx_transfer_fn = {
|
||||||
|
.read = st_lsm6dsx_i2c_read,
|
||||||
|
.write = st_lsm6dsx_i2c_write,
|
||||||
|
};
|
||||||
|
|
||||||
|
static int st_lsm6dsx_i2c_probe(struct i2c_client *client,
|
||||||
|
const struct i2c_device_id *id)
|
||||||
|
{
|
||||||
|
return st_lsm6dsx_probe(&client->dev, client->irq,
|
||||||
|
(int)id->driver_data,
|
||||||
|
&st_lsm6dsx_transfer_fn);
|
||||||
|
}
|
||||||
|
|
||||||
|
static const struct of_device_id st_lsm6dsx_i2c_of_match[] = {
|
||||||
|
{
|
||||||
|
.compatible = "st,lsm6ds3",
|
||||||
|
.data = (void *)ST_LSM6DS3_ID,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
.compatible = "st,lsm6dsm",
|
||||||
|
.data = (void *)ST_LSM6DSM_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_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
|
||||||
|
{},
|
||||||
|
};
|
||||||
|
MODULE_DEVICE_TABLE(i2c, st_lsm6dsx_i2c_id_table);
|
||||||
|
|
||||||
|
static struct i2c_driver st_lsm6dsx_driver = {
|
||||||
|
.driver = {
|
||||||
|
.name = "st_lsm6dsx_i2c",
|
||||||
|
.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");
|
||||||
118
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
Normal file
118
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
Normal file
|
|
@ -0,0 +1,118 @@
|
||||||
|
/*
|
||||||
|
* 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 "st_lsm6dsx.h"
|
||||||
|
|
||||||
|
#define SENSORS_SPI_READ BIT(7)
|
||||||
|
|
||||||
|
static int st_lsm6dsx_spi_read(struct device *dev, u8 addr, int len,
|
||||||
|
u8 *data)
|
||||||
|
{
|
||||||
|
struct spi_device *spi = to_spi_device(dev);
|
||||||
|
struct st_lsm6dsx_hw *hw = spi_get_drvdata(spi);
|
||||||
|
int err;
|
||||||
|
|
||||||
|
struct spi_transfer xfers[] = {
|
||||||
|
{
|
||||||
|
.tx_buf = hw->tb.tx_buf,
|
||||||
|
.bits_per_word = 8,
|
||||||
|
.len = 1,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
.rx_buf = hw->tb.rx_buf,
|
||||||
|
.bits_per_word = 8,
|
||||||
|
.len = len,
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
hw->tb.tx_buf[0] = addr | SENSORS_SPI_READ;
|
||||||
|
|
||||||
|
err = spi_sync_transfer(spi, xfers, ARRAY_SIZE(xfers));
|
||||||
|
if (err < 0)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
memcpy(data, hw->tb.rx_buf, len * sizeof(u8));
|
||||||
|
|
||||||
|
return len;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int st_lsm6dsx_spi_write(struct device *dev, u8 addr, int len,
|
||||||
|
u8 *data)
|
||||||
|
{
|
||||||
|
struct st_lsm6dsx_hw *hw;
|
||||||
|
struct spi_device *spi;
|
||||||
|
|
||||||
|
if (len >= ST_LSM6DSX_TX_MAX_LENGTH)
|
||||||
|
return -ENOMEM;
|
||||||
|
|
||||||
|
spi = to_spi_device(dev);
|
||||||
|
hw = spi_get_drvdata(spi);
|
||||||
|
|
||||||
|
hw->tb.tx_buf[0] = addr;
|
||||||
|
memcpy(&hw->tb.tx_buf[1], data, len);
|
||||||
|
|
||||||
|
return spi_write(spi, hw->tb.tx_buf, len + 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
static const struct st_lsm6dsx_transfer_function st_lsm6dsx_transfer_fn = {
|
||||||
|
.read = st_lsm6dsx_spi_read,
|
||||||
|
.write = st_lsm6dsx_spi_write,
|
||||||
|
};
|
||||||
|
|
||||||
|
static int st_lsm6dsx_spi_probe(struct spi_device *spi)
|
||||||
|
{
|
||||||
|
const struct spi_device_id *id = spi_get_device_id(spi);
|
||||||
|
|
||||||
|
return st_lsm6dsx_probe(&spi->dev, spi->irq,
|
||||||
|
(int)id->driver_data,
|
||||||
|
&st_lsm6dsx_transfer_fn);
|
||||||
|
}
|
||||||
|
|
||||||
|
static const struct of_device_id st_lsm6dsx_spi_of_match[] = {
|
||||||
|
{
|
||||||
|
.compatible = "st,lsm6ds3",
|
||||||
|
.data = (void *)ST_LSM6DS3_ID,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
.compatible = "st,lsm6dsm",
|
||||||
|
.data = (void *)ST_LSM6DSM_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_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
|
||||||
|
{},
|
||||||
|
};
|
||||||
|
MODULE_DEVICE_TABLE(spi, st_lsm6dsx_spi_id_table);
|
||||||
|
|
||||||
|
static struct spi_driver st_lsm6dsx_driver = {
|
||||||
|
.driver = {
|
||||||
|
.name = "st_lsm6dsx_spi",
|
||||||
|
.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");
|
||||||
Loading…
Add table
Reference in a new issue