lastdragon/linux-kernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Resguardo inicial

Browse Source
This commit is contained in:
lastdragon
2026-09-06 12:53:21 -06:00
parent 662efe3c2a
commit ad2088d178
94853 changed files with 43200404 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
linux-7.2.3/drivers/iio/imu/st_lsm6dsx/Kconfig
+57
View File
@@ -0,0 +1,57 @@
# SPDX-License-Identifier: GPL-2.0-only
config IIO_ST_LSM6DSX
tristate "ST_LSM6DSx driver for STM 6-axis IMU MEMS sensors"
depends on (I2C || SPI || I3C)
select IIO_BUFFER
select IIO_TRIGGERED_BUFFER
select IIO_KFIFO_BUF
help
Say yes here to build support for STMicroelectronics LSM6DSx imu
sensor.
Supported devices:
- asm330lhb
- asm330lhh
- asm330lhhx
- asm330lhhxg1
- ism330dhcx
- ism330dlc
- ism330is
- lsm6ds0
- lsm6ds3
- lsm6ds3h
- lsm6ds3tr-c
- lsm6dsl
- lsm6dsm
- lsm6dso
- lsm6dso16is
- lsm6dsop
- lsm6dsox
- lsm6dsr
- lsm6dsrx
- lsm6dst
- lsm6dstx
- lsm6dsv
- lsm6dsv16x
- lsm9ds1
To compile this driver as a module, choose M here: the module
will be called st_lsm6dsx.
config IIO_ST_LSM6DSX_I2C
tristate "ST_LSM6DSx driver for STM 6-axis IMU MEMS sensors I2C Interface"
depends on I2C && IIO_ST_LSM6DSX
default I2C && IIO_ST_LSM6DSX
select REGMAP_I2C
config IIO_ST_LSM6DSX_SPI
tristate "ST_LSM6DSx driver for STM 6-axis IMU MEMS sensors SPI Interface"
depends on SPI_MASTER && IIO_ST_LSM6DSX
default SPI_MASTER && IIO_ST_LSM6DSX
select REGMAP_SPI
config IIO_ST_LSM6DSX_I3C
tristate "ST_LSM6DSx driver for STM 6-axis IMU MEMS sensors I3C Interface"
depends on I3C && IIO_ST_LSM6DSX
default I3C && IIO_ST_LSM6DSX
select REGMAP_I3C
linux-7.2.3/drivers/iio/imu/st_lsm6dsx/Makefile
+8
View File
@@ -0,0 +1,8 @@
# SPDX-License-Identifier: GPL-2.0-only
st_lsm6dsx-y := st_lsm6dsx_core.o st_lsm6dsx_buffer.o \
st_lsm6dsx_shub.o st_lsm6dsx_fusion.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
obj-$(CONFIG_IIO_ST_LSM6DSX_I3C) += st_lsm6dsx_i3c.o
linux-7.2.3/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+599
View File
@@ -0,0 +1,599 @@
/* SPDX-License-Identifier: GPL-2.0-only */
/*
* STMicroelectronics st_lsm6dsx sensor driver
*
* Copyright 2016 STMicroelectronics Inc.
*
* Lorenzo Bianconi <lorenzo.bianconi@st.com>
* Denis Ciocca <denis.ciocca@st.com>
*/
#ifndef ST_LSM6DSX_H
#define ST_LSM6DSX_H
#include <linux/device.h>
#include <linux/iio/iio.h>
#include <linux/regulator/consumer.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"
#define ST_LSM6DSO_DEV_NAME "lsm6dso"
#define ST_ASM330LHH_DEV_NAME "asm330lhh"
#define ST_LSM6DSOX_DEV_NAME "lsm6dsox"
#define ST_LSM6DSR_DEV_NAME "lsm6dsr"
#define ST_LSM6DS3TRC_DEV_NAME "lsm6ds3tr-c"
#define ST_ISM330DHCX_DEV_NAME "ism330dhcx"
#define ST_LSM9DS1_DEV_NAME "lsm9ds1-imu"
#define ST_LSM6DS0_DEV_NAME "lsm6ds0"
#define ST_LSM6DSRX_DEV_NAME "lsm6dsrx"
#define ST_LSM6DST_DEV_NAME "lsm6dst"
#define ST_LSM6DSOP_DEV_NAME "lsm6dsop"
#define ST_ASM330LHHX_DEV_NAME "asm330lhhx"
#define ST_LSM6DSTX_DEV_NAME "lsm6dstx"
#define ST_LSM6DSV_DEV_NAME "lsm6dsv"
#define ST_LSM6DSV16X_DEV_NAME "lsm6dsv16x"
#define ST_LSM6DSO16IS_DEV_NAME "lsm6dso16is"
#define ST_ISM330IS_DEV_NAME "ism330is"
#define ST_ASM330LHB_DEV_NAME "asm330lhb"
#define ST_ASM330LHHXG1_DEV_NAME "asm330lhhxg1"
enum st_lsm6dsx_hw_id {
ST_LSM6DS3_ID = 1,
ST_LSM6DS3H_ID,
ST_LSM6DSL_ID,
ST_LSM6DSM_ID,
ST_ISM330DLC_ID,
ST_LSM6DSO_ID,
ST_ASM330LHH_ID,
ST_LSM6DSOX_ID,
ST_LSM6DSR_ID,
ST_LSM6DS3TRC_ID,
ST_ISM330DHCX_ID,
ST_LSM9DS1_ID,
ST_LSM6DS0_ID,
ST_LSM6DSRX_ID,
ST_LSM6DST_ID,
ST_LSM6DSOP_ID,
ST_ASM330LHHX_ID,
ST_LSM6DSTX_ID,
ST_LSM6DSV_ID,
ST_LSM6DSV16X_ID,
ST_LSM6DSO16IS_ID,
ST_ISM330IS_ID,
ST_ASM330LHB_ID,
ST_ASM330LHHXG1_ID,
ST_LSM6DSX_MAX_ID,
};
#define ST_LSM6DSX_BUFF_SIZE 512
#define ST_LSM6DSX_CHAN_SIZE 2
#define ST_LSM6DSX_SAMPLE_SIZE 6
#define ST_LSM6DSX_TAG_SIZE 1
#define ST_LSM6DSX_TAGGED_SAMPLE_SIZE (ST_LSM6DSX_SAMPLE_SIZE + \
ST_LSM6DSX_TAG_SIZE)
#define ST_LSM6DSX_MAX_WORD_LEN ((32 / ST_LSM6DSX_SAMPLE_SIZE) * \
ST_LSM6DSX_SAMPLE_SIZE)
#define ST_LSM6DSX_MAX_TAGGED_WORD_LEN ((32 / ST_LSM6DSX_TAGGED_SAMPLE_SIZE) \
* ST_LSM6DSX_TAGGED_SAMPLE_SIZE)
#define ST_LSM6DSX_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask))
#define st_lsm6dsx_field_get(mask, reg) ((reg & mask) >> __ffs(mask))
#define ST_LSM6DSX_CHANNEL_ACC(addr, mod, scan_idx, events) \
{ \
.type = IIO_ACCEL, \
.address = addr, \
.modified = 1, \
.channel2 = mod, \
.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
.info_mask_shared_by_type = 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, \
}, \
.event_spec = events, \
.num_event_specs = ARRAY_SIZE(events), \
.ext_info = st_lsm6dsx_ext_info, \
}
#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), \
.info_mask_shared_by_type = 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, \
}, \
.ext_info = st_lsm6dsx_ext_info, \
}
struct st_lsm6dsx_reg {
u8 addr;
u8 mask;
};
struct st_lsm6dsx_sensor;
struct st_lsm6dsx_hw;
struct st_lsm6dsx_odr {
u32 milli_hz;
u8 val;
};
#define ST_LSM6DSX_ODR_LIST_SIZE 8
struct st_lsm6dsx_odr_table_entry {
struct st_lsm6dsx_reg reg;
struct st_lsm6dsx_odr odr_avl[ST_LSM6DSX_ODR_LIST_SIZE];
int odr_len;
};
struct st_lsm6dsx_samples_to_discard {
struct {
u32 milli_hz;
u16 samples;
} val[ST_LSM6DSX_ODR_LIST_SIZE];
};
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];
int fs_len;
};
/**
* struct st_lsm6dsx_fifo_ops - ST IMU FIFO settings
* @update_fifo: Update FIFO configuration callback.
* @read_fifo: Read FIFO callback.
* @fifo_th: FIFO threshold register info (addr + mask).
* @fifo_diff: FIFO diff status register info (addr + mask).
* @max_size: Sensor max fifo length in FIFO words.
* @th_wl: FIFO threshold word length.
*/
struct st_lsm6dsx_fifo_ops {
int (*update_fifo)(struct st_lsm6dsx_sensor *sensor, bool enable);
int (*read_fifo)(struct st_lsm6dsx_hw *hw);
struct {
u8 addr;
u16 mask;
} fifo_th;
struct {
u8 addr;
u16 mask;
} fifo_diff;
u16 max_size;
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).
* @freq_fine: Difference in % of ODR with respect to the typical.
* @ts_sensitivity: Nominal timestamp sensitivity.
* @ts_trim_coeff: Coefficient for calculating the calibrated timestamp gain.
* This coefficient comes into play when linearizing the formula
* used to calculate the calibrated timestamp (please see the
* relevant formula in the AN for the specific IMU).
* For example, in the case of LSM6DSO we have:
*
* 1 / (1 + x) ~= 1 - x (Taylor’s Series)
* ttrim[s] = 1 / (40000 * (1 + 0.0015 * val)) (from AN5192)
* ttrim[ns] ~= 25000 - 37.5 * val
* ttrim[ns] ~= 25000 - (37500 * val) / 1000
*
* so, replacing ts_sensitivity = 25000 and
* ts_trim_coeff = 37500
*
* ttrim[ns] ~= ts_sensitivity - (ts_trim_coeff * val) / 1000
*/
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;
u8 freq_fine;
u16 ts_sensitivity;
u16 ts_trim_coeff;
};
/**
* struct st_lsm6dsx_shub_settings - ST IMU hw i2c controller settings
* @page_mux: register page mux info (addr + mask).
* @master_en: master config register info (addr + mask).
* @pullup_en: i2c controller pull-up register info (addr + mask).
* @aux_sens: aux sensor register info (addr + mask).
* @wr_once: write_once register info (addr + mask).
* @emb_func: embedded function register info (addr + mask).
* @num_ext_dev: max number of slave devices.
* @shub_out: sensor hub first output register info.
* @slv0_addr: slave0 address in secondary page.
* @dw_slv0_addr: slave0 write register address in secondary page.
* @batch_en: Enable/disable FIFO batching.
* @pause: controller pause value.
*/
struct st_lsm6dsx_shub_settings {
struct st_lsm6dsx_reg page_mux;
struct {
bool sec_page;
u8 addr;
u8 mask;
} master_en;
struct {
bool sec_page;
u8 addr;
u8 mask;
} pullup_en;
struct st_lsm6dsx_reg aux_sens;
struct st_lsm6dsx_reg wr_once;
struct st_lsm6dsx_reg emb_func;
u8 num_ext_dev;
struct {
bool sec_page;
u8 addr;
} shub_out;
u8 slv0_addr;
u8 dw_slv0_addr;
u8 batch_en;
u8 pause;
};
enum st_lsm6dsx_event_id {
ST_LSM6DSX_EVENT_WAKEUP,
ST_LSM6DSX_EVENT_TAP,
ST_LSM6DSX_EVENT_MAX
};
struct st_lsm6dsx_event_src {
struct st_lsm6dsx_reg value;
struct st_lsm6dsx_reg x_value;
struct st_lsm6dsx_reg y_value;
struct st_lsm6dsx_reg z_value;
u8 enable_mask;
u8 enable_axis_reg;
u8 enable_x_mask;
u8 enable_y_mask;
u8 enable_z_mask;
struct st_lsm6dsx_reg status;
u8 status_x_mask;
u8 status_y_mask;
u8 status_z_mask;
};
struct st_lsm6dsx_event_settings {
struct st_lsm6dsx_reg enable_reg;
struct st_lsm6dsx_event_src sources[ST_LSM6DSX_EVENT_MAX];
};
enum st_lsm6dsx_sensor_id {
ST_LSM6DSX_ID_GYRO,
ST_LSM6DSX_ID_ACC,
ST_LSM6DSX_ID_EXT0,
ST_LSM6DSX_ID_EXT1,
ST_LSM6DSX_ID_EXT2,
ST_LSM6DSX_ID_FUSION,
ST_LSM6DSX_ID_MAX
};
enum st_lsm6dsx_ext_sensor_id {
ST_LSM6DSX_ID_MAGN,
};
struct st_lsm6dsx_fusion_settings {
const struct iio_chan_spec *chan;
int chan_len;
struct st_lsm6dsx_reg odr_reg;
int odr_hz[ST_LSM6DSX_ODR_LIST_SIZE];
int odr_len;
struct st_lsm6dsx_reg fifo_enable;
struct st_lsm6dsx_reg page_mux;
struct st_lsm6dsx_reg enable;
};
/**
* struct st_lsm6dsx_ext_dev_settings - i2c controller slave settings
* @i2c_addr: I2c slave address list.
* @wai: Wai address info.
* @id: external sensor id.
* @odr_table: Output data rate of the sensor [Hz].
* @fs_table: Configured sensor sensitivity table depending on full scale.
* @temp_comp: Temperature compensation register info (addr + mask).
* @pwr_table: Power on register info (addr + mask).
* @off_canc: Offset cancellation register info (addr + mask).
* @bdu: Block data update register info (addr + mask).
* @out: Output register info.
*/
struct st_lsm6dsx_ext_dev_settings {
u8 i2c_addr[2];
struct {
u8 addr;
u8 val;
} wai;
enum st_lsm6dsx_ext_sensor_id id;
struct st_lsm6dsx_odr_table_entry odr_table;
struct st_lsm6dsx_fs_table_entry fs_table;
struct st_lsm6dsx_reg temp_comp;
struct {
struct st_lsm6dsx_reg reg;
u8 off_val;
u8 on_val;
} pwr_table;
struct st_lsm6dsx_reg off_canc;
struct st_lsm6dsx_reg bdu;
struct {
u8 addr;
u8 len;
} out;
};
/**
* struct st_lsm6dsx_settings - ST IMU sensor settings
* @reset: register address for reset.
* @boot: register address for boot.
* @bdu: register address for Block Data Update.
* @id: List of hw id/device name supported by the driver configuration.
* @channels: IIO channels supported by the device.
* @irq_config: interrupts related registers.
* @drdy_mask: register info for data-ready mask (addr + mask).
* @odr_table: Hw sensors odr table (Hz + val).
* @samples_to_discard: Number of samples to discard for filters settling time.
* @fs_table: Hw sensors gain table (gain + val).
* @decimator: List of decimator register info (addr + mask).
* @batch: List of FIFO batching register info (addr + mask).
* @fifo_ops: Sensor hw FIFO parameters.
* @ts_settings: Hw timer related settings.
* @shub_settings: i2c controller related settings.
*/
struct st_lsm6dsx_settings {
struct st_lsm6dsx_reg reset;
struct st_lsm6dsx_reg boot;
struct st_lsm6dsx_reg bdu;
struct {
enum st_lsm6dsx_hw_id hw_id;
const char *name;
u8 wai;
} id[ST_LSM6DSX_MAX_ID];
struct {
const struct iio_chan_spec *chan;
int len;
} channels[2];
struct {
struct st_lsm6dsx_reg irq1;
struct st_lsm6dsx_reg irq2;
u8 irq1_func;
u8 irq2_func;
struct st_lsm6dsx_reg lir;
struct st_lsm6dsx_reg clear_on_read;
struct st_lsm6dsx_reg hla;
struct st_lsm6dsx_reg od;
} irq_config;
struct st_lsm6dsx_reg drdy_mask;
struct st_lsm6dsx_odr_table_entry odr_table[2];
struct st_lsm6dsx_samples_to_discard samples_to_discard[2];
struct st_lsm6dsx_fs_table_entry fs_table[2];
struct st_lsm6dsx_reg decimator[ST_LSM6DSX_ID_MAX];
struct st_lsm6dsx_reg batch[2];
struct st_lsm6dsx_fifo_ops fifo_ops;
struct st_lsm6dsx_hw_ts_settings ts_settings;
struct st_lsm6dsx_shub_settings shub_settings;
struct st_lsm6dsx_event_settings event_settings;
struct st_lsm6dsx_fusion_settings fusion_settings;
};
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 [mHz].
* hwfifo_odr_mHz: Batch data rate for hardware FIFO [mHz]
* @samples_to_discard: Number of samples to discard for filters settling time.
* @watermark: Sensor watermark level.
* @decimator: Sensor decimation factor.
* @sip: Number of samples in a given pattern.
* @ts_ref: Sensor timestamp reference for hw one.
* @ext_info: Sensor settings if it is connected to i2c controller
*/
struct st_lsm6dsx_sensor {
char name[32];
enum st_lsm6dsx_sensor_id id;
struct st_lsm6dsx_hw *hw;
u32 gain;
u32 odr;
u32 hwfifo_odr_mHz;
u16 samples_to_discard;
u16 watermark;
u8 decimator;
u8 sip;
s64 ts_ref;
struct {
const struct st_lsm6dsx_ext_dev_settings *settings;
u32 slv_odr;
u8 addr;
} ext_info;
};
/**
* 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.
* @page_lock: Mutex to prevent concurrent memory page configuration.
* @suspend_mask: Suspended sensor bitmask.
* @enable_mask: Enabled sensor bitmask.
* @fifo_mask: Enabled hw FIFO bitmask.
* @ts_gain: Hw timestamp rate after internal calibration.
* @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.
* @irq_routing: pointer to interrupt routing configuration.
* @enable_event: enabled event bitmask.
* @iio_devs: Pointers to acc/gyro iio_dev instances.
* @settings: Pointer to the specific sensor settings in use.
* @orientation: sensor chip orientation relative to main hardware.
* @scan: Temporary buffers used to align data before iio_push_to_buffers()
*/
struct st_lsm6dsx_hw {
struct device *dev;
struct regmap *regmap;
int irq;
struct mutex fifo_lock;
struct mutex conf_lock;
struct mutex page_lock;
u8 suspend_mask;
u8 enable_mask;
u8 fifo_mask;
s64 ts_gain;
u8 ts_sip;
u8 sip;
u8 irq_routing;
u8 enable_event[ST_LSM6DSX_EVENT_MAX];
u8 *buff;
struct iio_dev *iio_devs[ST_LSM6DSX_ID_MAX];
const struct st_lsm6dsx_settings *settings;
struct iio_mount_matrix orientation;
/* Ensure natural alignment of buffer elements */
struct {
__le16 channels[3];
aligned_s64 ts;
} scan[ST_LSM6DSX_ID_MAX];
};
static __maybe_unused const unsigned long st_lsm6dsx_available_scan_masks[] = {
0x7, 0x0,
};
extern const struct dev_pm_ops st_lsm6dsx_pm_ops;
int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id,
struct regmap *regmap);
int st_lsm6dsx_sensor_set_enable(struct st_lsm6dsx_sensor *sensor,
bool enable);
int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw);
int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val);
int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor,
u16 watermark);
int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable);
int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw);
int st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw *hw);
int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw);
int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw);
int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u32 odr, u8 *val);
int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name);
int st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable);
int st_lsm6dsx_shub_read_output(struct st_lsm6dsx_hw *hw, u8 *data, int len);
int st_lsm6dsx_fusion_probe(struct st_lsm6dsx_hw *hw, const char *name);
int st_lsm6dsx_fusion_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable);
int st_lsm6dsx_fusion_set_odr(struct st_lsm6dsx_sensor *sensor, bool enable);
int st_lsm6dsx_set_page(struct st_lsm6dsx_hw *hw, bool enable);
static inline int
st_lsm6dsx_update_bits_locked(struct st_lsm6dsx_hw *hw, unsigned int addr,
unsigned int mask, unsigned int val)
{
int err;
mutex_lock(&hw->page_lock);
err = regmap_update_bits(hw->regmap, addr, mask, val);
mutex_unlock(&hw->page_lock);
return err;
}
static inline int
st_lsm6dsx_read_locked(struct st_lsm6dsx_hw *hw, unsigned int addr,
void *val, unsigned int len)
{
int err;
mutex_lock(&hw->page_lock);
err = regmap_bulk_read(hw->regmap, addr, val, len);
mutex_unlock(&hw->page_lock);
return err;
}
static inline int
st_lsm6dsx_write_locked(struct st_lsm6dsx_hw *hw, unsigned int addr,
unsigned int val)
{
int err;
mutex_lock(&hw->page_lock);
err = regmap_write(hw->regmap, addr, val);
mutex_unlock(&hw->page_lock);
return err;
}
static inline const struct iio_mount_matrix *
st_lsm6dsx_get_mount_matrix(const struct iio_dev *iio_dev,
const struct iio_chan_spec *chan)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
struct st_lsm6dsx_hw *hw = sensor->hw;
return &hw->orientation;
}
static inline int
st_lsm6dsx_device_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable)
{
switch (sensor->id) {
case ST_LSM6DSX_ID_EXT0 ... ST_LSM6DSX_ID_EXT2:
return st_lsm6dsx_shub_set_enable(sensor, enable);
case ST_LSM6DSX_ID_FUSION:
return st_lsm6dsx_fusion_set_enable(sensor, enable);
default:
return st_lsm6dsx_sensor_set_enable(sensor, enable);
}
}
static const
struct iio_chan_spec_ext_info __maybe_unused st_lsm6dsx_ext_info[] = {
IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, st_lsm6dsx_get_mount_matrix),
{ }
};
#endif /* ST_LSM6DSX_H */
linux-7.2.3/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+904
View File
@@ -0,0 +1,904 @@
// SPDX-License-Identifier: GPL-2.0-only
/*
* STMicroelectronics st_lsm6dsx FIFO buffer library driver
*
* Pattern FIFO:
* 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.
*
* Supported devices:
* - ISM330DLC
* - LSM6DS3
* - LSM6DS3H
* - LSM6DS3TR-C
* - LSM6DSL
* - LSM6DSM
*
* Tagged FIFO:
* The FIFO buffer can be configured to store data from gyroscope and
* accelerometer. Each sample is queued with a tag (1B) indicating data
* source (gyroscope, accelerometer, hw timer).
*
* Supported devices:
* - ASM330LHB
* - ASM330LHH
* - ASM330LHHX
* - ASM330LHHXG1
* - ISM330DHCX
* - LSM6DSO
* - LSM6DSOP
* - LSM6DSOX
* - LSM6DSR
* - LSM6DSRX
* - LSM6DST
* - LSM6DSTX
* - LSM6DSV
*
* 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>
*/
#include <linux/module.h>
#include <linux/iio/kfifo_buf.h>
#include <linux/iio/iio.h>
#include <linux/iio/buffer.h>
#include <linux/iio/sysfs.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_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_FIFO_OUT_TAG_ADDR 0x78
#define ST_LSM6DSX_REG_TS_RESET_ADDR 0x42
#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
#define ST_LSM6DSX_TS_RESET_VAL 0xaa
struct st_lsm6dsx_decimator_entry {
u8 decimator;
u8 val;
};
enum st_lsm6dsx_fifo_tag {
ST_LSM6DSX_GYRO_TAG = 0x01,
ST_LSM6DSX_ACC_TAG = 0x02,
ST_LSM6DSX_TS_TAG = 0x04,
ST_LSM6DSX_EXT0_TAG = 0x0f,
ST_LSM6DSX_EXT1_TAG = 0x10,
ST_LSM6DSX_EXT2_TAG = 0x11,
ST_LSM6DSX_ROT_TAG = 0x13,
};
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(struct st_lsm6dsx_sensor *sensor, u32 max_odr)
{
const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
u32 decimator = max_odr / sensor->hwfifo_odr_mHz;
int i;
if (decimator > 1)
decimator = round_down(decimator, 2);
for (i = 0; i < max_size; i++) {
if (st_lsm6dsx_decimator_table[i].decimator == decimator)
break;
}
sensor->decimator = decimator;
return i == max_size ? 0 : st_lsm6dsx_decimator_table[i].val;
}
static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
u32 *max_odr, u32 *min_odr)
{
struct st_lsm6dsx_sensor *sensor;
int i;
*max_odr = 0, *min_odr = ~0;
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
if (!hw->iio_devs[i])
continue;
sensor = iio_priv(hw->iio_devs[i]);
if (!(hw->enable_mask & BIT(sensor->id)))
continue;
*max_odr = max(*max_odr, sensor->hwfifo_odr_mHz);
*min_odr = min(*min_odr, sensor->hwfifo_odr_mHz);
}
}
static u8 st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor *sensor, u32 min_odr)
{
u8 sip = sensor->hwfifo_odr_mHz / min_odr;
return sip > 1 ? round_down(sip, 2) : sip;
}
static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
{
const struct st_lsm6dsx_reg *ts_dec_reg;
struct st_lsm6dsx_sensor *sensor;
u16 sip = 0, ts_sip = 0;
u32 max_odr, min_odr;
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;
if (!hw->iio_devs[i])
continue;
sensor = iio_priv(hw->iio_devs[i]);
/* update fifo decimators and sample in pattern */
if (hw->enable_mask & BIT(sensor->id)) {
sensor->sip = st_lsm6dsx_get_sip(sensor, min_odr);
data = st_lsm6dsx_get_decimator_val(sensor, max_odr);
} else {
sensor->sip = 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 = st_lsm6dsx_update_bits_locked(hw, 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 = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr,
ts_dec_reg->mask, val);
}
return err;
}
static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
enum st_lsm6dsx_fifo_mode fifo_mode)
{
unsigned int data;
data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
return st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
ST_LSM6DSX_FIFO_MODE_MASK, data);
}
static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
bool enable)
{
struct st_lsm6dsx_hw *hw = sensor->hw;
const struct st_lsm6dsx_reg *batch_reg;
u8 data;
/* Only internal sensors have a FIFO ODR configuration register. */
if (sensor->id >= ARRAY_SIZE(hw->settings->batch)) {
if (sensor->id == ST_LSM6DSX_ID_FUSION)
return st_lsm6dsx_fusion_set_odr(sensor, enable);
return 0;
}
batch_reg = &hw->settings->batch[sensor->id];
if (batch_reg->addr) {
int val;
if (enable) {
int err;
err = st_lsm6dsx_check_odr(sensor, sensor->hwfifo_odr_mHz,
&data);
if (err < 0)
return err;
} else {
data = 0;
}
val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask);
return st_lsm6dsx_update_bits_locked(hw, batch_reg->addr,
batch_reg->mask, val);
} else {
data = hw->enable_mask ? ST_LSM6DSX_MAX_FIFO_ODR_VAL : 0;
return st_lsm6dsx_update_bits_locked(hw,
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++) {
if (!hw->iio_devs[i])
continue;
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;
mutex_lock(&hw->page_lock);
err = regmap_read(hw->regmap, hw->settings->fifo_ops.fifo_th.addr + 1,
&data);
if (err < 0)
goto out;
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);
err = regmap_bulk_write(hw->regmap,
hw->settings->fifo_ops.fifo_th.addr,
&wdata, sizeof(wdata));
out:
mutex_unlock(&hw->page_lock);
return err;
}
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 = st_lsm6dsx_write_locked(hw, 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++) {
if (!hw->iio_devs[i])
continue;
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;
}
int st_lsm6dsx_resume_fifo(struct st_lsm6dsx_hw *hw)
{
int err;
/* reset hw ts counter */
err = st_lsm6dsx_reset_hw_ts(hw);
if (err < 0)
return err;
return st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
}
/*
* Set max bulk read to ST_LSM6DSX_MAX_WORD_LEN/ST_LSM6DSX_MAX_TAGGED_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 addr,
u8 *data, unsigned int data_len,
unsigned int max_word_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,
max_word_len);
err = st_lsm6dsx_read_locked(hw, addr, data + read_len,
word_len);
if (err < 0)
return err;
read_len += word_len;
}
return 0;
}
/**
* 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
*/
int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
{
struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor, *ext_sensor = NULL;
int err, sip, acc_sip, gyro_sip, ts_sip, ext_sip, read_len, offset;
u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
u16 fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
bool reset_ts = false;
__le16 fifo_status;
s64 ts = 0;
err = st_lsm6dsx_read_locked(hw,
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;
if (!pattern_len)
pattern_len = ST_LSM6DSX_SAMPLE_SIZE;
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]);
if (hw->iio_devs[ST_LSM6DSX_ID_EXT0])
ext_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_EXT0]);
for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
err = st_lsm6dsx_read_block(hw, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
hw->buff, pattern_len,
ST_LSM6DSX_MAX_WORD_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, ..
*/
ext_sip = ext_sensor ? ext_sensor->sip : 0;
gyro_sip = gyro_sensor->sip;
acc_sip = acc_sensor->sip;
ts_sip = hw->ts_sip;
offset = 0;
sip = 0;
while (acc_sip > 0 || gyro_sip > 0 || ext_sip > 0) {
if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
memcpy(hw->scan[ST_LSM6DSX_ID_GYRO].channels,
&hw->buff[offset],
sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels));
offset += sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels);
}
if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
memcpy(hw->scan[ST_LSM6DSX_ID_ACC].channels,
&hw->buff[offset],
sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels));
offset += sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels);
}
if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
memcpy(hw->scan[ST_LSM6DSX_ID_EXT0].channels,
&hw->buff[offset],
sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels));
offset += sizeof(hw->scan[ST_LSM6DSX_ID_EXT0].channels);
}
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 *= hw->ts_gain;
offset += ST_LSM6DSX_SAMPLE_SIZE;
}
if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
/*
* We need to discards gyro samples during
* filters settling time
*/
if (gyro_sensor->samples_to_discard > 0)
gyro_sensor->samples_to_discard--;
else
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_GYRO],
&hw->scan[ST_LSM6DSX_ID_GYRO],
gyro_sensor->ts_ref + ts);
gyro_sip--;
}
if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
/*
* We need to discards accel samples during
* filters settling time
*/
if (acc_sensor->samples_to_discard > 0)
acc_sensor->samples_to_discard--;
else
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_ACC],
&hw->scan[ST_LSM6DSX_ID_ACC],
acc_sensor->ts_ref + ts);
acc_sip--;
}
if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
iio_push_to_buffers_with_timestamp(
hw->iio_devs[ST_LSM6DSX_ID_EXT0],
&hw->scan[ST_LSM6DSX_ID_EXT0],
ext_sensor->ts_ref + ts);
ext_sip--;
}
sip++;
}
}
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;
}
#define ST_LSM6DSX_INVALID_SAMPLE 0x7ffd
static bool st_lsm6dsx_check_data(u8 tag, __le16 *data)
{
if ((tag == ST_LSM6DSX_GYRO_TAG || tag == ST_LSM6DSX_ACC_TAG) &&
(s16)le16_to_cpup(data) >= ST_LSM6DSX_INVALID_SAMPLE)
return false;
return true;
}
static int st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
__le16 *data, s64 ts)
{
struct st_lsm6dsx_sensor *sensor;
struct iio_dev *iio_dev;
/* invalid sample during bootstrap phase */
if (!st_lsm6dsx_check_data(tag, data))
return -EINVAL;
/*
* EXT_TAG are managed in FIFO fashion so ST_LSM6DSX_EXT0_TAG
* corresponds to the first enabled channel, ST_LSM6DSX_EXT1_TAG
* to the second one and ST_LSM6DSX_EXT2_TAG to the last enabled
* channel
*/
switch (tag) {
case ST_LSM6DSX_GYRO_TAG:
iio_dev = hw->iio_devs[ST_LSM6DSX_ID_GYRO];
break;
case ST_LSM6DSX_ACC_TAG:
iio_dev = hw->iio_devs[ST_LSM6DSX_ID_ACC];
break;
case ST_LSM6DSX_EXT0_TAG:
if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0))
iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT0];
else if (hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1))
iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
else
iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
break;
case ST_LSM6DSX_EXT1_TAG:
if ((hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT0)) &&
(hw->enable_mask & BIT(ST_LSM6DSX_ID_EXT1)))
iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT1];
else
iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
break;
case ST_LSM6DSX_EXT2_TAG:
iio_dev = hw->iio_devs[ST_LSM6DSX_ID_EXT2];
break;
case ST_LSM6DSX_ROT_TAG:
iio_dev = hw->iio_devs[ST_LSM6DSX_ID_FUSION];
break;
default:
return -EINVAL;
}
sensor = iio_priv(iio_dev);
iio_push_to_buffers_with_timestamp(iio_dev, data,
ts + sensor->ts_ref);
return 0;
}
/**
* st_lsm6dsx_read_tagged_fifo() - tagged 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
*/
int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
{
u16 pattern_len = hw->sip * ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
u16 fifo_len, fifo_diff_mask;
/*
* Alignment needed as this can ultimately be passed to a
* call to iio_push_to_buffers_with_timestamp() which
* must be passed a buffer that is aligned to 8 bytes so
* as to allow insertion of a naturally aligned timestamp.
*/
struct {
union {
__le16 data[3];
__le32 fifo_ts;
};
aligned_s64 timestamp;
} iio_buff = { };
u8 tag;
bool reset_ts = false;
int i, err, read_len;
__le16 fifo_status;
s64 ts = 0;
err = st_lsm6dsx_read_locked(hw,
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;
}
fifo_diff_mask = hw->settings->fifo_ops.fifo_diff.mask;
fifo_len = (le16_to_cpu(fifo_status) & fifo_diff_mask) *
ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
if (!fifo_len)
return 0;
if (!pattern_len)
pattern_len = ST_LSM6DSX_TAGGED_SAMPLE_SIZE;
for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
err = st_lsm6dsx_read_block(hw,
ST_LSM6DSX_REG_FIFO_OUT_TAG_ADDR,
hw->buff, pattern_len,
ST_LSM6DSX_MAX_TAGGED_WORD_LEN);
if (err < 0) {
dev_err(hw->dev,
"failed to read pattern from fifo (err=%d)\n",
err);
return err;
}
for (i = 0; i < pattern_len;
i += ST_LSM6DSX_TAGGED_SAMPLE_SIZE) {
memcpy(&iio_buff, &hw->buff[i + ST_LSM6DSX_TAG_SIZE],
ST_LSM6DSX_SAMPLE_SIZE);
tag = hw->buff[i] >> 3;
if (tag == ST_LSM6DSX_TS_TAG) {
/*
* hw timestamp is 4B long and it is stored
* in FIFO according to this schema:
* B0 = ts[7:0], B1 = ts[15:8], B2 = ts[23:16],
* B3 = ts[31:24]
*/
ts = le32_to_cpu(iio_buff.fifo_ts);
/*
* 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 >= 0xffff0000)
reset_ts = true;
ts *= hw->ts_gain;
} else {
st_lsm6dsx_push_tagged_data(hw, tag,
iio_buff.data,
ts);
}
}
}
if (unlikely(reset_ts)) {
err = st_lsm6dsx_reset_hw_ts(hw);
if (err < 0)
return err;
}
return read_len;
}
int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
{
int err;
if (!hw->settings->fifo_ops.read_fifo)
return -ENOTSUPP;
mutex_lock(&hw->fifo_lock);
hw->settings->fifo_ops.read_fifo(hw);
err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
mutex_unlock(&hw->fifo_lock);
return err;
}
static void
st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor *sensor)
{
const struct st_lsm6dsx_samples_to_discard *data;
struct st_lsm6dsx_hw *hw = sensor->hw;
int i;
if (sensor->id != ST_LSM6DSX_ID_GYRO &&
sensor->id != ST_LSM6DSX_ID_ACC)
return;
/* check if drdy mask is supported in hw */
if (hw->settings->drdy_mask.addr)
return;
data = &hw->settings->samples_to_discard[sensor->id];
for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) {
if (data->val[i].milli_hz == sensor->hwfifo_odr_mHz) {
sensor->samples_to_discard = data->val[i].samples;
return;
}
}
}
int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
{
struct st_lsm6dsx_hw *hw = sensor->hw;
u8 fifo_mask;
int err;
mutex_lock(&hw->conf_lock);
if (enable)
fifo_mask = hw->fifo_mask | BIT(sensor->id);
else
fifo_mask = hw->fifo_mask & ~BIT(sensor->id);
if (hw->fifo_mask) {
err = st_lsm6dsx_flush_fifo(hw);
if (err < 0)
goto out;
}
if (enable)
st_lsm6dsx_update_samples_to_discard(sensor);
err = st_lsm6dsx_device_set_enable(sensor, enable);
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 (fifo_mask) {
err = st_lsm6dsx_resume_fifo(hw);
if (err < 0)
goto out;
}
hw->fifo_mask = fifo_mask;
out:
mutex_unlock(&hw->conf_lock);
return err;
}
static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
struct st_lsm6dsx_hw *hw = sensor->hw;
if (!hw->settings->fifo_ops.update_fifo)
return -ENOTSUPP;
return hw->settings->fifo_ops.update_fifo(sensor, true);
}
static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
struct st_lsm6dsx_hw *hw = sensor->hw;
if (!hw->settings->fifo_ops.update_fifo)
return -ENOTSUPP;
return hw->settings->fifo_ops.update_fifo(sensor, false);
}
static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
.preenable = st_lsm6dsx_buffer_preenable,
.postdisable = st_lsm6dsx_buffer_postdisable,
};
static ssize_t st_lsm6dsx_hwfifo_odr_show(struct device *dev,
struct device_attribute *attr, char *buf)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(dev_to_iio_dev(dev));
return sysfs_emit(buf, "%d.%03d\n", sensor->hwfifo_odr_mHz / 1000,
sensor->hwfifo_odr_mHz % 1000);
}
static ssize_t st_lsm6dsx_hwfifo_odr_store(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t len)
{
struct iio_dev *iio_dev = dev_to_iio_dev(dev);
struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
int integer, milli;
int ret;
u32 hwfifo_odr;
u8 data;
if (!iio_device_claim_direct(iio_dev))
return -EBUSY;
ret = iio_str_to_fixpoint(buf, 100, &integer, &milli);
if (ret)
goto out;
hwfifo_odr = integer * 1000 + milli;
ret = st_lsm6dsx_check_odr(sensor, hwfifo_odr, &data);
if (ret < 0)
goto out;
hwfifo_odr = ret;
/* the batch data rate must not exceed the sensor output data rate */
if (hwfifo_odr <= sensor->odr)
sensor->hwfifo_odr_mHz = hwfifo_odr;
else
ret = -EINVAL;
out:
iio_device_release_direct(iio_dev);
return ret < 0 ? ret : len;
}
static IIO_DEV_ATTR_SAMP_FREQ(0664, st_lsm6dsx_hwfifo_odr_show, st_lsm6dsx_hwfifo_odr_store);
static const struct iio_dev_attr *st_lsm6dsx_buffer_attrs[] = {
&iio_dev_attr_sampling_frequency,
NULL
};
int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
{
int i, ret;
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
const struct iio_dev_attr **attrs;
if (!hw->iio_devs[i])
continue;
/*
* For the accelerometer, allow setting FIFO sampling frequency
* values different from the sensor sampling frequency, which
* may be needed to keep FIFO data rate low while sampling
* acceleration data at high rates for accurate event detection.
*/
attrs = i == ST_LSM6DSX_ID_ACC ? st_lsm6dsx_buffer_attrs : NULL;
ret = devm_iio_kfifo_buffer_setup_ext(hw->dev, hw->iio_devs[i],
&st_lsm6dsx_buffer_ops,
attrs);
if (ret)
return ret;
}
return 0;
}
linux-7.2.3/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+3096
View File
File diff suppressed because it is too large Load Diff
linux-7.2.3/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c
+241
View File
@@ -0,0 +1,241 @@
// SPDX-License-Identifier: GPL-2.0-only
/*
* STMicroelectronics st_lsm6dsx IMU sensor fusion
*
* Copyright 2026 BayLibre, SAS
*/
#include <linux/cleanup.h>
#include <linux/errno.h>
#include <linux/iio/iio.h>
#include <linux/iio/sysfs.h>
#include <linux/mutex.h>
#include <linux/regmap.h>
#include <linux/sprintf.h>
#include <linux/types.h>
#include <linux/units.h>
#include "st_lsm6dsx.h"
static int
st_lsm6dsx_fusion_get_odr_val(const struct st_lsm6dsx_fusion_settings *settings,
u32 odr_mHz, u8 *val)
{
int odr_hz = odr_mHz / MILLI;
int i;
for (i = 0; i < settings->odr_len; i++) {
if (settings->odr_hz[i] == odr_hz)
break;
}
if (i == settings->odr_len)
return -EINVAL;
*val = i;
return 0;
}
/**
* st_lsm6dsx_fusion_page_enable - Enable access to sensor fusion configuration
* registers.
* @hw: Sensor hardware instance.
*
* Return: 0 on success, negative value on error.
*/
static int st_lsm6dsx_fusion_page_enable(struct st_lsm6dsx_hw *hw)
{
const struct st_lsm6dsx_reg *mux;
mux = &hw->settings->fusion_settings.page_mux;
return regmap_set_bits(hw->regmap, mux->addr, mux->mask);
}
/**
* st_lsm6dsx_fusion_page_disable - Disable access to sensor fusion
* configuration registers.
* @hw: Sensor hardware instance.
*
* Return: 0 on success, negative value on error.
*/
static int st_lsm6dsx_fusion_page_disable(struct st_lsm6dsx_hw *hw)
{
const struct st_lsm6dsx_reg *mux;
mux = &hw->settings->fusion_settings.page_mux;
return regmap_clear_bits(hw->regmap, mux->addr, mux->mask);
}
static int st_lsm6dsx_fusion_set_odr_locked(struct st_lsm6dsx_sensor *sensor,
bool enable)
{
const struct st_lsm6dsx_fusion_settings *settings;
struct st_lsm6dsx_hw *hw = sensor->hw;
int err;
settings = &hw->settings->fusion_settings;
if (enable) {
const struct st_lsm6dsx_reg *reg = &settings->odr_reg;
u8 odr_val;
u8 data;
st_lsm6dsx_fusion_get_odr_val(settings, sensor->hwfifo_odr_mHz,
&odr_val);
data = ST_LSM6DSX_SHIFT_VAL(odr_val, reg->mask);
err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
data);
if (err)
return err;
}
return regmap_assign_bits(hw->regmap, settings->fifo_enable.addr,
settings->fifo_enable.mask, enable);
}
int st_lsm6dsx_fusion_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable)
{
struct st_lsm6dsx_hw *hw = sensor->hw;
const struct st_lsm6dsx_reg *en_reg;
int err;
guard(mutex)(&hw->page_lock);
en_reg = &hw->settings->fusion_settings.enable;
err = st_lsm6dsx_fusion_page_enable(hw);
if (err)
return err;
err = regmap_assign_bits(hw->regmap, en_reg->addr, en_reg->mask, enable);
if (err) {
st_lsm6dsx_fusion_page_disable(hw);
return err;
}
return st_lsm6dsx_fusion_page_disable(hw);
}
int st_lsm6dsx_fusion_set_odr(struct st_lsm6dsx_sensor *sensor, bool enable)
{
struct st_lsm6dsx_hw *hw = sensor->hw;
int err;
guard(mutex)(&hw->page_lock);
err = st_lsm6dsx_fusion_page_enable(hw);
if (err)
return err;
err = st_lsm6dsx_fusion_set_odr_locked(sensor, enable);
if (err) {
st_lsm6dsx_fusion_page_disable(hw);
return err;
}
return st_lsm6dsx_fusion_page_disable(hw);
}
static int st_lsm6dsx_fusion_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);
switch (mask) {
case IIO_CHAN_INFO_SAMP_FREQ:
*val = sensor->hwfifo_odr_mHz / MILLI;
*val2 = (sensor->hwfifo_odr_mHz % MILLI) * (MICRO / MILLI);
return IIO_VAL_INT_PLUS_MICRO;
default:
return -EINVAL;
}
}
static int st_lsm6dsx_fusion_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);
const struct st_lsm6dsx_fusion_settings *settings;
int err;
settings = &sensor->hw->settings->fusion_settings;
switch (mask) {
case IIO_CHAN_INFO_SAMP_FREQ: {
u32 odr_mHz = val * MILLI + val2 * (MILLI / MICRO);
u8 odr_val;
/* check that the requested frequency is supported */
err = st_lsm6dsx_fusion_get_odr_val(settings, odr_mHz, &odr_val);
if (err)
return err;
sensor->hwfifo_odr_mHz = odr_mHz;
return 0;
}
default:
return -EINVAL;
}
}
static int st_lsm6dsx_fusion_read_avail(struct iio_dev *indio_dev,
struct iio_chan_spec const *chan,
const int **vals, int *type,
int *length, long mask)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(indio_dev);
const struct st_lsm6dsx_fusion_settings *settings;
settings = &sensor->hw->settings->fusion_settings;
switch (mask) {
case IIO_CHAN_INFO_SAMP_FREQ:
*vals = settings->odr_hz;
*type = IIO_VAL_INT;
*length = settings->odr_len;
return IIO_AVAIL_LIST;
default:
return -EINVAL;
}
}
static const struct iio_info st_lsm6dsx_fusion_info = {
.read_raw = st_lsm6dsx_fusion_read_raw,
.read_avail = st_lsm6dsx_fusion_read_avail,
.write_raw = st_lsm6dsx_fusion_write_raw,
.hwfifo_set_watermark = st_lsm6dsx_set_watermark,
};
int st_lsm6dsx_fusion_probe(struct st_lsm6dsx_hw *hw, const char *name)
{
const struct st_lsm6dsx_fusion_settings *settings;
struct st_lsm6dsx_sensor *sensor;
struct iio_dev *iio_dev;
int ret;
iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
if (!iio_dev)
return -ENOMEM;
settings = &hw->settings->fusion_settings;
sensor = iio_priv(iio_dev);
sensor->id = ST_LSM6DSX_ID_FUSION;
sensor->hw = hw;
sensor->hwfifo_odr_mHz = settings->odr_hz[0] * MILLI;
sensor->watermark = 1;
iio_dev->modes = INDIO_DIRECT_MODE;
iio_dev->info = &st_lsm6dsx_fusion_info;
iio_dev->channels = settings->chan;
iio_dev->num_channels = settings->chan_len;
ret = snprintf(sensor->name, sizeof(sensor->name), "%s_fusion", name);
if (ret >= sizeof(sensor->name))
return -E2BIG;
iio_dev->name = sensor->name;
/*
* Put the IIO device pointer in the iio_devs array so that the caller
* can set up a buffer and register this IIO device.
*/
hw->iio_devs[ST_LSM6DSX_ID_FUSION] = iio_dev;
return 0;
}
linux-7.2.3/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
+197
View File
@@ -0,0 +1,197 @@
// SPDX-License-Identifier: GPL-2.0-only
/*
* STMicroelectronics st_lsm6dsx i2c driver
*
* Copyright 2016 STMicroelectronics Inc.
*
* Lorenzo Bianconi <lorenzo.bianconi@st.com>
* Denis Ciocca <denis.ciocca@st.com>
*/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/i2c.h>
#include <linux/slab.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)
{
int hw_id;
struct regmap *regmap;
hw_id = (kernel_ulong_t)device_get_match_data(&client->dev);
if (!hw_id)
hw_id = i2c_client_get_device_id(client)->driver_data;
if (!hw_id)
return -EINVAL;
regmap = devm_regmap_init_i2c(client, &st_lsm6dsx_i2c_regmap_config);
if (IS_ERR(regmap)) {
dev_err(&client->dev, "Failed to register i2c regmap %ld\n", PTR_ERR(regmap));
return PTR_ERR(regmap);
}
return st_lsm6dsx_probe(&client->dev, client->irq, hw_id, 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,
},
{
.compatible = "st,lsm6dso",
.data = (void *)ST_LSM6DSO_ID,
},
{
.compatible = "st,asm330lhh",
.data = (void *)ST_ASM330LHH_ID,
},
{
.compatible = "st,lsm6dsox",
.data = (void *)ST_LSM6DSOX_ID,
},
{
.compatible = "st,lsm6dsr",
.data = (void *)ST_LSM6DSR_ID,
},
{
.compatible = "st,lsm6ds3tr-c",
.data = (void *)ST_LSM6DS3TRC_ID,
},
{
.compatible = "st,ism330dhcx",
.data = (void *)ST_ISM330DHCX_ID,
},
{
.compatible = "st,lsm9ds1-imu",
.data = (void *)ST_LSM9DS1_ID,
},
{
.compatible = "st,lsm6ds0",
.data = (void *)ST_LSM6DS0_ID,
},
{
.compatible = "st,lsm6dsrx",
.data = (void *)ST_LSM6DSRX_ID,
},
{
.compatible = "st,lsm6dst",
.data = (void *)ST_LSM6DST_ID,
},
{
.compatible = "st,lsm6dsop",
.data = (void *)ST_LSM6DSOP_ID,
},
{
.compatible = "st,asm330lhhx",
.data = (void *)ST_ASM330LHHX_ID,
},
{
.compatible = "st,lsm6dstx",
.data = (void *)ST_LSM6DSTX_ID,
},
{
.compatible = "st,lsm6dsv",
.data = (void *)ST_LSM6DSV_ID,
},
{
.compatible = "st,lsm6dsv16x",
.data = (void *)ST_LSM6DSV16X_ID,
},
{
.compatible = "st,lsm6dso16is",
.data = (void *)ST_LSM6DSO16IS_ID,
},
{
.compatible = "st,ism330is",
.data = (void *)ST_ISM330IS_ID,
},
{
.compatible = "st,asm330lhb",
.data = (void *)ST_ASM330LHB_ID,
},
{
.compatible = "st,asm330lhhxg1",
.data = (void *)ST_ASM330LHHXG1_ID,
},
{ }
};
MODULE_DEVICE_TABLE(of, st_lsm6dsx_i2c_of_match);
static const struct acpi_device_id st_lsm6dsx_i2c_acpi_match[] = {
{ "SMO8B30", ST_LSM6DS3TRC_ID, },
{ "SMOCF00", ST_LSM6DSO_ID, },
{ }
};
MODULE_DEVICE_TABLE(acpi, st_lsm6dsx_i2c_acpi_match);
static const struct i2c_device_id st_lsm6dsx_i2c_id_table[] = {
{ .name = ST_LSM6DS3_DEV_NAME, .driver_data = ST_LSM6DS3_ID },
{ .name = ST_LSM6DS3H_DEV_NAME, .driver_data = ST_LSM6DS3H_ID },
{ .name = ST_LSM6DSL_DEV_NAME, .driver_data = ST_LSM6DSL_ID },
{ .name = ST_LSM6DSM_DEV_NAME, .driver_data = ST_LSM6DSM_ID },
{ .name = ST_ISM330DLC_DEV_NAME, .driver_data = ST_ISM330DLC_ID },
{ .name = ST_LSM6DSO_DEV_NAME, .driver_data = ST_LSM6DSO_ID },
{ .name = ST_ASM330LHH_DEV_NAME, .driver_data = ST_ASM330LHH_ID },
{ .name = ST_LSM6DSOX_DEV_NAME, .driver_data = ST_LSM6DSOX_ID },
{ .name = ST_LSM6DSR_DEV_NAME, .driver_data = ST_LSM6DSR_ID },
{ .name = ST_LSM6DS3TRC_DEV_NAME, .driver_data = ST_LSM6DS3TRC_ID },
{ .name = ST_ISM330DHCX_DEV_NAME, .driver_data = ST_ISM330DHCX_ID },
{ .name = ST_LSM9DS1_DEV_NAME, .driver_data = ST_LSM9DS1_ID },
{ .name = ST_LSM6DS0_DEV_NAME, .driver_data = ST_LSM6DS0_ID },
{ .name = ST_LSM6DSRX_DEV_NAME, .driver_data = ST_LSM6DSRX_ID },
{ .name = ST_LSM6DST_DEV_NAME, .driver_data = ST_LSM6DST_ID },
{ .name = ST_LSM6DSOP_DEV_NAME, .driver_data = ST_LSM6DSOP_ID },
{ .name = ST_ASM330LHHX_DEV_NAME, .driver_data = ST_ASM330LHHX_ID },
{ .name = ST_LSM6DSTX_DEV_NAME, .driver_data = ST_LSM6DSTX_ID },
{ .name = ST_LSM6DSV_DEV_NAME, .driver_data = ST_LSM6DSV_ID },
{ .name = ST_LSM6DSV16X_DEV_NAME, .driver_data = ST_LSM6DSV16X_ID },
{ .name = ST_LSM6DSO16IS_DEV_NAME, .driver_data = ST_LSM6DSO16IS_ID },
{ .name = ST_ISM330IS_DEV_NAME, .driver_data = ST_ISM330IS_ID },
{ .name = ST_ASM330LHB_DEV_NAME, .driver_data = ST_ASM330LHB_ID },
{ .name = ST_ASM330LHHXG1_DEV_NAME, .driver_data = ST_ASM330LHHXG1_ID },
{ }
};
MODULE_DEVICE_TABLE(i2c, st_lsm6dsx_i2c_id_table);
static struct i2c_driver st_lsm6dsx_driver = {
.driver = {
.name = "st_lsm6dsx_i2c",
.pm = pm_sleep_ptr(&st_lsm6dsx_pm_ops),
.of_match_table = st_lsm6dsx_i2c_of_match,
.acpi_match_table = st_lsm6dsx_i2c_acpi_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");
MODULE_IMPORT_NS("IIO_LSM6DSX");
linux-7.2.3/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i3c.c
+56
View File
@@ -0,0 +1,56 @@
// SPDX-License-Identifier: GPL-2.0
/*
* Copyright (c) 2018 Synopsys, Inc. and/or its affiliates.
*
* Author: Vitor Soares <vitor.soares@synopsys.com>
*/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/i3c/device.h>
#include <linux/slab.h>
#include <linux/regmap.h>
#include "st_lsm6dsx.h"
static const struct i3c_device_id st_lsm6dsx_i3c_ids[] = {
I3C_DEVICE(0x0104, 0x006C, (void *)ST_LSM6DSO_ID),
I3C_DEVICE(0x0104, 0x006B, (void *)ST_LSM6DSR_ID),
{ }
};
MODULE_DEVICE_TABLE(i3c, st_lsm6dsx_i3c_ids);
static int st_lsm6dsx_i3c_probe(struct i3c_device *i3cdev)
{
struct regmap_config st_lsm6dsx_i3c_regmap_config = {
.reg_bits = 8,
.val_bits = 8,
};
const struct i3c_device_id *id = i3c_device_match_id(i3cdev,
st_lsm6dsx_i3c_ids);
struct device *dev = i3cdev_to_dev(i3cdev);
struct regmap *regmap;
regmap = devm_regmap_init_i3c(i3cdev, &st_lsm6dsx_i3c_regmap_config);
if (IS_ERR(regmap)) {
dev_err(dev, "Failed to register i3c regmap %ld\n", PTR_ERR(regmap));
return PTR_ERR(regmap);
}
return st_lsm6dsx_probe(dev, 0, (uintptr_t)id->data, regmap);
}
static struct i3c_driver st_lsm6dsx_driver = {
.driver = {
.name = "st_lsm6dsx_i3c",
.pm = pm_sleep_ptr(&st_lsm6dsx_pm_ops),
},
.probe = st_lsm6dsx_i3c_probe,
.id_table = st_lsm6dsx_i3c_ids,
};
module_i3c_driver(st_lsm6dsx_driver);
MODULE_AUTHOR("Vitor Soares <vitor.soares@synopsys.com>");
MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx i3c driver");
MODULE_LICENSE("GPL v2");
MODULE_IMPORT_NS("IIO_LSM6DSX");
linux-7.2.3/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
+922
View File
@@ -0,0 +1,922 @@
/*
* STMicroelectronics st_lsm6dsx i2c controller driver
*
* i2c controller embedded in lsm6dx series can connect up to four
* slave devices using accelerometer sensor as trigger for i2c
* read/write operations. Current implementation relies on SLV0 channel
* for slave configuration and SLV{1,2,3} to read data and push them into
* the hw FIFO
*
* Copyright (C) 2018 Lorenzo Bianconi <lorenzo.bianconi83@gmail.com>
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
* ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
* ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
* OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*
*/
#include <linux/module.h>
#include <linux/regmap.h>
#include <linux/iio/iio.h>
#include <linux/iio/sysfs.h>
#include <linux/bitfield.h>
#include "st_lsm6dsx.h"
#define ST_LSM6DSX_SLV_ADDR(n, base) ((base) + (n) * 3)
#define ST_LSM6DSX_SLV_SUB_ADDR(n, base) ((base) + 1 + (n) * 3)
#define ST_LSM6DSX_SLV_CONFIG(n, base) ((base) + 2 + (n) * 3)
#define ST_LS6DSX_READ_OP_MASK GENMASK(2, 0)
static const struct st_lsm6dsx_ext_dev_settings st_lsm6dsx_ext_dev_table[] = {
/* LIS2MDL */
{
.i2c_addr = { 0x1e },
.wai = {
.addr = 0x4f,
.val = 0x40,
},
.id = ST_LSM6DSX_ID_MAGN,
.odr_table = {
.reg = {
.addr = 0x60,
.mask = GENMASK(3, 2),
},
.odr_avl[0] = { 10000, 0x0 },
.odr_avl[1] = { 20000, 0x1 },
.odr_avl[2] = { 50000, 0x2 },
.odr_avl[3] = { 100000, 0x3 },
.odr_len = 4,
},
.fs_table = {
.fs_avl[0] = {
.gain = 1500,
.val = 0x0,
}, /* 1500 uG/LSB */
.fs_len = 1,
},
.temp_comp = {
.addr = 0x60,
.mask = BIT(7),
},
.pwr_table = {
.reg = {
.addr = 0x60,
.mask = GENMASK(1, 0),
},
.off_val = 0x2,
.on_val = 0x0,
},
.off_canc = {
.addr = 0x61,
.mask = BIT(1),
},
.bdu = {
.addr = 0x62,
.mask = BIT(4),
},
.out = {
.addr = 0x68,
.len = 6,
},
},
/* LIS3MDL */
{
.i2c_addr = { 0x1e },
.wai = {
.addr = 0x0f,
.val = 0x3d,
},
.id = ST_LSM6DSX_ID_MAGN,
.odr_table = {
.reg = {
.addr = 0x20,
.mask = GENMASK(4, 2),
},
.odr_avl[0] = { 1000, 0x0 },
.odr_avl[1] = { 2000, 0x1 },
.odr_avl[2] = { 3000, 0x2 },
.odr_avl[3] = { 5000, 0x3 },
.odr_avl[4] = { 10000, 0x4 },
.odr_avl[5] = { 20000, 0x5 },
.odr_avl[6] = { 40000, 0x6 },
.odr_avl[7] = { 80000, 0x7 },
.odr_len = 8,
},
.fs_table = {
.reg = {
.addr = 0x21,
.mask = GENMASK(6, 5),
},
.fs_avl[0] = {
.gain = 146,
.val = 0x00,
}, /* 4000 uG/LSB */
.fs_avl[1] = {
.gain = 292,
.val = 0x01,
}, /* 8000 uG/LSB */
.fs_avl[2] = {
.gain = 438,
.val = 0x02,
}, /* 12000 uG/LSB */
.fs_avl[3] = {
.gain = 584,
.val = 0x03,
}, /* 16000 uG/LSB */
.fs_len = 4,
},
.pwr_table = {
.reg = {
.addr = 0x22,
.mask = GENMASK(1, 0),
},
.off_val = 0x2,
.on_val = 0x0,
},
.bdu = {
.addr = 0x24,
.mask = BIT(6),
},
.out = {
.addr = 0x28,
.len = 6,
},
},
};
static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw)
{
struct st_lsm6dsx_sensor *sensor;
u32 odr, timeout;
sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
odr = (hw->enable_mask & BIT(ST_LSM6DSX_ID_ACC)) ? sensor->odr : 12500;
/* set 10ms as minimum timeout for i2c slave configuration */
timeout = max_t(u32, 2000000U / odr + 1, 10);
msleep(timeout);
}
/*
* st_lsm6dsx_shub_read_output - read i2c controller register
*
* Read st_lsm6dsx i2c controller register
*/
int st_lsm6dsx_shub_read_output(struct st_lsm6dsx_hw *hw, u8 *data, int len)
{
const struct st_lsm6dsx_shub_settings *hub_settings;
int err;
mutex_lock(&hw->page_lock);
hub_settings = &hw->settings->shub_settings;
if (hub_settings->shub_out.sec_page) {
err = st_lsm6dsx_set_page(hw, true);
if (err < 0)
goto out;
}
err = regmap_bulk_read(hw->regmap, hub_settings->shub_out.addr,
data, len);
if (hub_settings->shub_out.sec_page)
st_lsm6dsx_set_page(hw, false);
out:
mutex_unlock(&hw->page_lock);
return err;
}
/*
* st_lsm6dsx_shub_write_reg - write i2c controller register
*
* Write st_lsm6dsx i2c controller register
*/
static int st_lsm6dsx_shub_write_reg(struct st_lsm6dsx_hw *hw, u8 addr,
u8 *data, int len)
{
int err;
mutex_lock(&hw->page_lock);
err = st_lsm6dsx_set_page(hw, true);
if (err < 0)
goto out;
err = regmap_bulk_write(hw->regmap, addr, data, len);
st_lsm6dsx_set_page(hw, false);
out:
mutex_unlock(&hw->page_lock);
return err;
}
static int
st_lsm6dsx_shub_write_reg_with_mask(struct st_lsm6dsx_hw *hw, u8 addr,
u8 mask, u8 val)
{
int err;
mutex_lock(&hw->page_lock);
err = st_lsm6dsx_set_page(hw, true);
if (err < 0)
goto out;
err = regmap_update_bits(hw->regmap, addr, mask, val);
st_lsm6dsx_set_page(hw, false);
out:
mutex_unlock(&hw->page_lock);
return err;
}
static int st_lsm6dsx_shub_master_enable(struct st_lsm6dsx_sensor *sensor,
bool enable)
{
const struct st_lsm6dsx_shub_settings *hub_settings;
struct st_lsm6dsx_hw *hw = sensor->hw;
unsigned int data;
int err;
/* enable acc sensor as trigger */
err = st_lsm6dsx_sensor_set_enable(sensor, enable);
if (err < 0)
return err;
mutex_lock(&hw->page_lock);
hub_settings = &hw->settings->shub_settings;
if (hub_settings->master_en.sec_page) {
err = st_lsm6dsx_set_page(hw, true);
if (err < 0)
goto out;
}
data = ST_LSM6DSX_SHIFT_VAL(enable, hub_settings->master_en.mask);
err = regmap_update_bits(hw->regmap, hub_settings->master_en.addr,
hub_settings->master_en.mask, data);
if (hub_settings->master_en.sec_page)
st_lsm6dsx_set_page(hw, false);
out:
mutex_unlock(&hw->page_lock);
return err;
}
/*
* st_lsm6dsx_shub_read - read data from slave device register
*
* Read data from slave device register. SLV0 is used for
* one-shot read operation
*/
static int
st_lsm6dsx_shub_read(struct st_lsm6dsx_sensor *sensor, u8 addr,
u8 *data, int len)
{
const struct st_lsm6dsx_shub_settings *hub_settings;
u8 config[3], slv_addr, slv_config = 0;
struct st_lsm6dsx_hw *hw = sensor->hw;
const struct st_lsm6dsx_reg *aux_sens;
int err;
hub_settings = &hw->settings->shub_settings;
slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
aux_sens = &hw->settings->shub_settings.aux_sens;
/* do not overwrite aux_sens */
if (slv_addr + 2 == aux_sens->addr)
slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
config[0] = (sensor->ext_info.addr << 1) | 1;
config[1] = addr;
config[2] = (len & ST_LS6DSX_READ_OP_MASK) | slv_config;
err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
sizeof(config));
if (err < 0)
return err;
err = st_lsm6dsx_shub_master_enable(sensor, true);
if (err < 0)
return err;
st_lsm6dsx_shub_wait_complete(hw);
err = st_lsm6dsx_shub_read_output(hw, data,
len & ST_LS6DSX_READ_OP_MASK);
if (err < 0)
return err;
st_lsm6dsx_shub_master_enable(sensor, false);
config[0] = hub_settings->pause;
config[1] = 0;
config[2] = slv_config;
return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
sizeof(config));
}
/*
* st_lsm6dsx_shub_write - write data to slave device register
*
* Write data from slave device register. SLV0 is used for
* one-shot write operation
*/
static int
st_lsm6dsx_shub_write(struct st_lsm6dsx_sensor *sensor, u8 addr,
u8 *data, int len)
{
const struct st_lsm6dsx_shub_settings *hub_settings;
struct st_lsm6dsx_hw *hw = sensor->hw;
u8 config[2], slv_addr;
int err, i;
hub_settings = &hw->settings->shub_settings;
if (hub_settings->wr_once.addr) {
unsigned int data;
data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->wr_once.mask);
err = st_lsm6dsx_shub_write_reg_with_mask(hw,
hub_settings->wr_once.addr,
hub_settings->wr_once.mask,
data);
if (err < 0)
return err;
}
slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
config[0] = sensor->ext_info.addr << 1;
for (i = 0 ; i < len; i++) {
config[1] = addr + i;
err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
sizeof(config));
if (err < 0)
return err;
err = st_lsm6dsx_shub_write_reg(hw, hub_settings->dw_slv0_addr,
&data[i], 1);
if (err < 0)
return err;
err = st_lsm6dsx_shub_master_enable(sensor, true);
if (err < 0)
return err;
st_lsm6dsx_shub_wait_complete(hw);
st_lsm6dsx_shub_master_enable(sensor, false);
}
config[0] = hub_settings->pause;
config[1] = 0;
return st_lsm6dsx_shub_write_reg(hw, slv_addr, config, sizeof(config));
}
static int
st_lsm6dsx_shub_write_with_mask(struct st_lsm6dsx_sensor *sensor,
u8 addr, u8 mask, u8 val)
{
int err;
u8 data;
err = st_lsm6dsx_shub_read(sensor, addr, &data, sizeof(data));
if (err < 0)
return err;
data = ((data & ~mask) | (val << __ffs(mask) & mask));
return st_lsm6dsx_shub_write(sensor, addr, &data, sizeof(data));
}
static int
st_lsm6dsx_shub_get_odr_val(struct st_lsm6dsx_sensor *sensor,
u32 odr, u16 *val)
{
const struct st_lsm6dsx_ext_dev_settings *settings;
int i;
settings = sensor->ext_info.settings;
for (i = 0; i < settings->odr_table.odr_len; i++) {
if (settings->odr_table.odr_avl[i].milli_hz == odr)
break;
}
if (i == settings->odr_table.odr_len)
return -EINVAL;
*val = settings->odr_table.odr_avl[i].val;
return 0;
}
static int
st_lsm6dsx_shub_set_odr(struct st_lsm6dsx_sensor *sensor, u32 odr)
{
const struct st_lsm6dsx_ext_dev_settings *settings;
u16 val;
int err;
err = st_lsm6dsx_shub_get_odr_val(sensor, odr, &val);
if (err < 0)
return err;
settings = sensor->ext_info.settings;
return st_lsm6dsx_shub_write_with_mask(sensor,
settings->odr_table.reg.addr,
settings->odr_table.reg.mask,
val);
}
/* use SLV{1,2,3} for FIFO read operations */
static int
st_lsm6dsx_shub_config_channels(struct st_lsm6dsx_sensor *sensor,
bool enable)
{
const struct st_lsm6dsx_shub_settings *hub_settings;
const struct st_lsm6dsx_ext_dev_settings *settings;
u8 config[9] = {}, enable_mask, slv_addr;
struct st_lsm6dsx_hw *hw = sensor->hw;
struct st_lsm6dsx_sensor *cur_sensor;
int i, j = 0;
hub_settings = &hw->settings->shub_settings;
if (enable)
enable_mask = hw->enable_mask | BIT(sensor->id);
else
enable_mask = hw->enable_mask & ~BIT(sensor->id);
for (i = ST_LSM6DSX_ID_EXT0; i <= ST_LSM6DSX_ID_EXT2; i++) {
if (!hw->iio_devs[i])
continue;
cur_sensor = iio_priv(hw->iio_devs[i]);
if (!(enable_mask & BIT(cur_sensor->id)))
continue;
settings = cur_sensor->ext_info.settings;
config[j] = (sensor->ext_info.addr << 1) | 1;
config[j + 1] = settings->out.addr;
config[j + 2] = (settings->out.len & ST_LS6DSX_READ_OP_MASK) |
hub_settings->batch_en;
j += 3;
}
slv_addr = ST_LSM6DSX_SLV_ADDR(1, hub_settings->slv0_addr);
return st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
sizeof(config));
}
int st_lsm6dsx_shub_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable)
{
const struct st_lsm6dsx_ext_dev_settings *settings;
int err;
err = st_lsm6dsx_shub_config_channels(sensor, enable);
if (err < 0)
return err;
settings = sensor->ext_info.settings;
if (enable) {
err = st_lsm6dsx_shub_set_odr(sensor,
sensor->ext_info.slv_odr);
if (err < 0)
return err;
} else {
err = st_lsm6dsx_shub_write_with_mask(sensor,
settings->odr_table.reg.addr,
settings->odr_table.reg.mask, 0);
if (err < 0)
return err;
}
if (settings->pwr_table.reg.addr) {
u8 val;
val = enable ? settings->pwr_table.on_val
: settings->pwr_table.off_val;
err = st_lsm6dsx_shub_write_with_mask(sensor,
settings->pwr_table.reg.addr,
settings->pwr_table.reg.mask, val);
if (err < 0)
return err;
}
return st_lsm6dsx_shub_master_enable(sensor, enable);
}
static int
st_lsm6dsx_shub_read_oneshot(struct st_lsm6dsx_sensor *sensor,
struct iio_chan_spec const *ch,
int *val)
{
int err, delay, len;
u8 data[4];
err = st_lsm6dsx_shub_set_enable(sensor, true);
if (err < 0)
return err;
delay = 1000000000 / sensor->ext_info.slv_odr;
usleep_range(delay, 2 * delay);
len = min_t(int, sizeof(data), ch->scan_type.realbits >> 3);
err = st_lsm6dsx_shub_read(sensor, ch->address, data, len);
if (err < 0)
return err;
err = st_lsm6dsx_shub_set_enable(sensor, false);
if (err < 0)
return err;
switch (len) {
case 2:
*val = (s16)le16_to_cpu(*((__le16 *)data));
break;
default:
return -EINVAL;
}
return IIO_VAL_INT;
}
static int
st_lsm6dsx_shub_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:
if (!iio_device_claim_direct(iio_dev))
return -EBUSY;
ret = st_lsm6dsx_shub_read_oneshot(sensor, ch, val);
iio_device_release_direct(iio_dev);
break;
case IIO_CHAN_INFO_SAMP_FREQ:
*val = sensor->ext_info.slv_odr / 1000;
*val2 = (sensor->ext_info.slv_odr % 1000) * 1000;
ret = IIO_VAL_INT_PLUS_MICRO;
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_shub_set_full_scale(struct st_lsm6dsx_sensor *sensor,
u32 gain)
{
const struct st_lsm6dsx_fs_table_entry *fs_table;
int i, err;
fs_table = &sensor->ext_info.settings->fs_table;
if (!fs_table->reg.addr)
return -ENOTSUPP;
for (i = 0; i < fs_table->fs_len; i++) {
if (fs_table->fs_avl[i].gain == gain)
break;
}
if (i == fs_table->fs_len)
return -EINVAL;
err = st_lsm6dsx_shub_write_with_mask(sensor, fs_table->reg.addr,
fs_table->reg.mask,
fs_table->fs_avl[i].val);
if (err < 0)
return err;
sensor->gain = gain;
return 0;
}
static int
__st_lsm6dsx_shub_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;
switch (mask) {
case IIO_CHAN_INFO_SAMP_FREQ: {
struct st_lsm6dsx_hw *hw = sensor->hw;
struct st_lsm6dsx_sensor *ref_sensor;
u8 odr_val;
u16 data;
int odr;
val = val * 1000 + val2 / 1000;
err = st_lsm6dsx_shub_get_odr_val(sensor, val, &data);
if (err)
return err;
ref_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
odr = st_lsm6dsx_check_odr(ref_sensor, val, &odr_val);
if (odr < 0)
return odr;
sensor->ext_info.slv_odr = val;
sensor->odr = odr;
sensor->hwfifo_odr_mHz = odr;
return 0;
}
case IIO_CHAN_INFO_SCALE:
return st_lsm6dsx_shub_set_full_scale(sensor, val2);
default:
return -EINVAL;
}
}
static int
st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev,
struct iio_chan_spec const *chan,
int val, int val2, long mask)
{
int ret;
if (!iio_device_claim_direct(iio_dev))
return -EBUSY;
ret = __st_lsm6dsx_shub_write_raw(iio_dev, chan, val, val2, mask);
iio_device_release_direct(iio_dev);
return ret;
}
static ssize_t
st_lsm6dsx_shub_sampling_freq_avail(struct device *dev,
struct device_attribute *attr,
char *buf)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
const struct st_lsm6dsx_ext_dev_settings *settings;
int i, len = 0;
settings = sensor->ext_info.settings;
for (i = 0; i < settings->odr_table.odr_len; i++) {
u32 val = settings->odr_table.odr_avl[i].milli_hz;
len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%03d ",
val / 1000, val % 1000);
}
buf[len - 1] = '\n';
return len;
}
static ssize_t st_lsm6dsx_shub_scale_avail(struct device *dev,
struct device_attribute *attr,
char *buf)
{
struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
const struct st_lsm6dsx_ext_dev_settings *settings;
int i, len = 0;
settings = sensor->ext_info.settings;
for (i = 0; i < settings->fs_table.fs_len; i++)
len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
settings->fs_table.fs_avl[i].gain);
buf[len - 1] = '\n';
return len;
}
static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_shub_sampling_freq_avail);
static IIO_DEVICE_ATTR(in_scale_available, 0444,
st_lsm6dsx_shub_scale_avail, NULL, 0);
static struct attribute *st_lsm6dsx_shub_attributes[] = {
&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
&iio_dev_attr_in_scale_available.dev_attr.attr,
NULL,
};
static const struct attribute_group st_lsm6dsx_shub_attribute_group = {
.attrs = st_lsm6dsx_shub_attributes,
};
static const struct iio_info st_lsm6dsx_shub_info = {
.attrs = &st_lsm6dsx_shub_attribute_group,
.read_raw = st_lsm6dsx_shub_read_raw,
.write_raw = st_lsm6dsx_shub_write_raw,
.hwfifo_set_watermark = st_lsm6dsx_set_watermark,
};
static struct iio_dev *
st_lsm6dsx_shub_alloc_iiodev(struct st_lsm6dsx_hw *hw,
enum st_lsm6dsx_sensor_id id,
const struct st_lsm6dsx_ext_dev_settings *info,
u8 i2c_addr, const char *name)
{
enum st_lsm6dsx_sensor_id ref_id = ST_LSM6DSX_ID_ACC;
struct iio_chan_spec *ext_channels;
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->info = &st_lsm6dsx_shub_info;
sensor = iio_priv(iio_dev);
sensor->id = id;
sensor->hw = hw;
sensor->odr = hw->settings->odr_table[ref_id].odr_avl[0].milli_hz;
sensor->hwfifo_odr_mHz = sensor->odr;
sensor->ext_info.slv_odr = info->odr_table.odr_avl[0].milli_hz;
sensor->gain = info->fs_table.fs_avl[0].gain;
sensor->ext_info.settings = info;
sensor->ext_info.addr = i2c_addr;
sensor->watermark = 1;
switch (info->id) {
case ST_LSM6DSX_ID_MAGN: {
const struct iio_chan_spec magn_channels[] = {
ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr,
IIO_MOD_X, 0),
ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 2,
IIO_MOD_Y, 1),
ST_LSM6DSX_CHANNEL(IIO_MAGN, info->out.addr + 4,
IIO_MOD_Z, 2),
IIO_CHAN_SOFT_TIMESTAMP(3),
};
ext_channels = devm_kzalloc(hw->dev, sizeof(magn_channels),
GFP_KERNEL);
if (!ext_channels)
return NULL;
memcpy(ext_channels, magn_channels, sizeof(magn_channels));
iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks;
iio_dev->channels = ext_channels;
iio_dev->num_channels = ARRAY_SIZE(magn_channels);
scnprintf(sensor->name, sizeof(sensor->name), "%s_magn",
name);
break;
}
default:
return NULL;
}
iio_dev->name = sensor->name;
return iio_dev;
}
static int st_lsm6dsx_shub_init_device(struct st_lsm6dsx_sensor *sensor)
{
const struct st_lsm6dsx_ext_dev_settings *settings;
int err;
settings = sensor->ext_info.settings;
if (settings->bdu.addr) {
err = st_lsm6dsx_shub_write_with_mask(sensor,
settings->bdu.addr,
settings->bdu.mask, 1);
if (err < 0)
return err;
}
if (settings->temp_comp.addr) {
err = st_lsm6dsx_shub_write_with_mask(sensor,
settings->temp_comp.addr,
settings->temp_comp.mask, 1);
if (err < 0)
return err;
}
if (settings->off_canc.addr) {
err = st_lsm6dsx_shub_write_with_mask(sensor,
settings->off_canc.addr,
settings->off_canc.mask, 1);
if (err < 0)
return err;
}
return 0;
}
static int
st_lsm6dsx_shub_check_wai(struct st_lsm6dsx_hw *hw, u8 *i2c_addr,
const struct st_lsm6dsx_ext_dev_settings *settings)
{
const struct st_lsm6dsx_shub_settings *hub_settings;
u8 config[3], data, slv_addr, slv_config = 0;
const struct st_lsm6dsx_reg *aux_sens;
struct st_lsm6dsx_sensor *sensor;
bool found = false;
int i, err;
sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
hub_settings = &hw->settings->shub_settings;
aux_sens = &hw->settings->shub_settings.aux_sens;
slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
/* do not overwrite aux_sens */
if (slv_addr + 2 == aux_sens->addr)
slv_config = ST_LSM6DSX_SHIFT_VAL(3, aux_sens->mask);
for (i = 0; i < ARRAY_SIZE(settings->i2c_addr); i++) {
if (!settings->i2c_addr[i])
continue;
/* read wai slave register */
config[0] = (settings->i2c_addr[i] << 1) | 0x1;
config[1] = settings->wai.addr;
config[2] = 0x1 | slv_config;
err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
sizeof(config));
if (err < 0)
return err;
err = st_lsm6dsx_shub_master_enable(sensor, true);
if (err < 0)
return err;
st_lsm6dsx_shub_wait_complete(hw);
err = st_lsm6dsx_shub_read_output(hw, &data, sizeof(data));
st_lsm6dsx_shub_master_enable(sensor, false);
if (err < 0)
return err;
if (data != settings->wai.val)
continue;
*i2c_addr = settings->i2c_addr[i];
found = true;
break;
}
/* reset SLV0 channel */
config[0] = hub_settings->pause;
config[1] = 0;
config[2] = slv_config;
err = st_lsm6dsx_shub_write_reg(hw, slv_addr, config,
sizeof(config));
if (err < 0)
return err;
return found ? 0 : -ENODEV;
}
int st_lsm6dsx_shub_probe(struct st_lsm6dsx_hw *hw, const char *name)
{
enum st_lsm6dsx_sensor_id id = ST_LSM6DSX_ID_EXT0;
struct st_lsm6dsx_sensor *sensor;
int err, i, num_ext_dev = 0;
u8 i2c_addr = 0;
for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_ext_dev_table); i++) {
err = st_lsm6dsx_shub_check_wai(hw, &i2c_addr,
&st_lsm6dsx_ext_dev_table[i]);
if (err == -ENODEV)
continue;
else if (err < 0)
return err;
hw->iio_devs[id] = st_lsm6dsx_shub_alloc_iiodev(hw, id,
&st_lsm6dsx_ext_dev_table[i],
i2c_addr, name);
if (!hw->iio_devs[id])
return -ENOMEM;
sensor = iio_priv(hw->iio_devs[id]);
err = st_lsm6dsx_shub_init_device(sensor);
if (err < 0)
return err;
if (++num_ext_dev >= hw->settings->shub_settings.num_ext_dev)
break;
id++;
}
return 0;
}
linux-7.2.3/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
+184
View File
@@ -0,0 +1,184 @@
// SPDX-License-Identifier: GPL-2.0-only
/*
* STMicroelectronics st_lsm6dsx spi driver
*
* Copyright 2016 STMicroelectronics Inc.
*
* Lorenzo Bianconi <lorenzo.bianconi@st.com>
* Denis Ciocca <denis.ciocca@st.com>
*/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/spi/spi.h>
#include <linux/slab.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 %ld\n", PTR_ERR(regmap));
return PTR_ERR(regmap);
}
return st_lsm6dsx_probe(&spi->dev, spi->irq, hw_id, 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,
},
{
.compatible = "st,lsm6dso",
.data = (void *)ST_LSM6DSO_ID,
},
{
.compatible = "st,asm330lhh",
.data = (void *)ST_ASM330LHH_ID,
},
{
.compatible = "st,lsm6dsox",
.data = (void *)ST_LSM6DSOX_ID,
},
{
.compatible = "st,lsm6dsr",
.data = (void *)ST_LSM6DSR_ID,
},
{
.compatible = "st,lsm6ds3tr-c",
.data = (void *)ST_LSM6DS3TRC_ID,
},
{
.compatible = "st,ism330dhcx",
.data = (void *)ST_ISM330DHCX_ID,
},
{
.compatible = "st,lsm9ds1-imu",
.data = (void *)ST_LSM9DS1_ID,
},
{
.compatible = "st,lsm6ds0",
.data = (void *)ST_LSM6DS0_ID,
},
{
.compatible = "st,lsm6dsrx",
.data = (void *)ST_LSM6DSRX_ID,
},
{
.compatible = "st,lsm6dst",
.data = (void *)ST_LSM6DST_ID,
},
{
.compatible = "st,lsm6dsop",
.data = (void *)ST_LSM6DSOP_ID,
},
{
.compatible = "st,asm330lhhx",
.data = (void *)ST_ASM330LHHX_ID,
},
{
.compatible = "st,lsm6dstx",
.data = (void *)ST_LSM6DSTX_ID,
},
{
.compatible = "st,lsm6dsv",
.data = (void *)ST_LSM6DSV_ID,
},
{
.compatible = "st,lsm6dsv16x",
.data = (void *)ST_LSM6DSV16X_ID,
},
{
.compatible = "st,lsm6dso16is",
.data = (void *)ST_LSM6DSO16IS_ID,
},
{
.compatible = "st,ism330is",
.data = (void *)ST_ISM330IS_ID,
},
{
.compatible = "st,asm330lhb",
.data = (void *)ST_ASM330LHB_ID,
},
{
.compatible = "st,asm330lhhxg1",
.data = (void *)ST_ASM330LHHXG1_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 },
{ ST_LSM6DSO_DEV_NAME, ST_LSM6DSO_ID },
{ ST_ASM330LHH_DEV_NAME, ST_ASM330LHH_ID },
{ ST_LSM6DSOX_DEV_NAME, ST_LSM6DSOX_ID },
{ ST_LSM6DSR_DEV_NAME, ST_LSM6DSR_ID },
{ ST_LSM6DS3TRC_DEV_NAME, ST_LSM6DS3TRC_ID },
{ ST_ISM330DHCX_DEV_NAME, ST_ISM330DHCX_ID },
{ ST_LSM9DS1_DEV_NAME, ST_LSM9DS1_ID },
{ ST_LSM6DS0_DEV_NAME, ST_LSM6DS0_ID },
{ ST_LSM6DSRX_DEV_NAME, ST_LSM6DSRX_ID },
{ ST_LSM6DST_DEV_NAME, ST_LSM6DST_ID },
{ ST_LSM6DSOP_DEV_NAME, ST_LSM6DSOP_ID },
{ ST_ASM330LHHX_DEV_NAME, ST_ASM330LHHX_ID },
{ ST_LSM6DSTX_DEV_NAME, ST_LSM6DSTX_ID },
{ ST_LSM6DSV_DEV_NAME, ST_LSM6DSV_ID },
{ ST_LSM6DSV16X_DEV_NAME, ST_LSM6DSV16X_ID },
{ ST_LSM6DSO16IS_DEV_NAME, ST_LSM6DSO16IS_ID },
{ ST_ISM330IS_DEV_NAME, ST_ISM330IS_ID },
{ ST_ASM330LHB_DEV_NAME, ST_ASM330LHB_ID },
{ ST_ASM330LHHXG1_DEV_NAME, ST_ASM330LHHXG1_ID },
{ }
};
MODULE_DEVICE_TABLE(spi, st_lsm6dsx_spi_id_table);
static struct spi_driver st_lsm6dsx_driver = {
.driver = {
.name = "st_lsm6dsx_spi",
.pm = pm_sleep_ptr(&st_lsm6dsx_pm_ops),
.of_match_table = 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");
MODULE_IMPORT_NS("IIO_LSM6DSX");