summaryrefslogtreecommitdiffstats
path: root/drivers/iio/imu
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/iio/imu')
-rw-r--r--drivers/iio/imu/bmi160/bmi160.h1
-rw-r--r--drivers/iio/imu/bmi160/bmi160_core.c38
-rw-r--r--drivers/iio/imu/bmi160/bmi160_i2c.c8
-rw-r--r--drivers/iio/imu/bmi160/bmi160_spi.c8
-rw-r--r--drivers/iio/imu/st_lsm6dsx/Makefile3
-rw-r--r--drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h167
-rw-r--r--drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c165
-rw-r--r--drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c283
-rw-r--r--drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c779
9 files changed, 1276 insertions, 176 deletions
diff --git a/drivers/iio/imu/bmi160/bmi160.h b/drivers/iio/imu/bmi160/bmi160.h
index e7b11e74fd1d..2351049d930b 100644
--- a/drivers/iio/imu/bmi160/bmi160.h
+++ b/drivers/iio/imu/bmi160/bmi160.h
@@ -6,6 +6,5 @@ extern const struct regmap_config bmi160_regmap_config;
int bmi160_core_probe(struct device *dev, struct regmap *regmap,
const char *name, bool use_spi);
-void bmi160_core_remove(struct device *dev);
#endif /* BMI160_H_ */
diff --git a/drivers/iio/imu/bmi160/bmi160_core.c b/drivers/iio/imu/bmi160/bmi160_core.c
index c85659ca9507..b10330b0f93f 100644
--- a/drivers/iio/imu/bmi160/bmi160_core.c
+++ b/drivers/iio/imu/bmi160/bmi160_core.c
@@ -542,10 +542,12 @@ static int bmi160_chip_init(struct bmi160_data *data, bool use_spi)
return 0;
}
-static void bmi160_chip_uninit(struct bmi160_data *data)
+static void bmi160_chip_uninit(void *data)
{
- bmi160_set_mode(data, BMI160_GYRO, false);
- bmi160_set_mode(data, BMI160_ACCEL, false);
+ struct bmi160_data *bmi_data = data;
+
+ bmi160_set_mode(bmi_data, BMI160_GYRO, false);
+ bmi160_set_mode(bmi_data, BMI160_ACCEL, false);
}
int bmi160_core_probe(struct device *dev, struct regmap *regmap,
@@ -567,6 +569,10 @@ int bmi160_core_probe(struct device *dev, struct regmap *regmap,
if (ret < 0)
return ret;
+ ret = devm_add_action_or_reset(dev, bmi160_chip_uninit, data);
+ if (ret < 0)
+ return ret;
+
if (!name && ACPI_HANDLE(dev))
name = bmi160_match_acpi_device(dev);
@@ -577,35 +583,19 @@ int bmi160_core_probe(struct device *dev, struct regmap *regmap,
indio_dev->modes = INDIO_DIRECT_MODE;
indio_dev->info = &bmi160_info;
- ret = iio_triggered_buffer_setup(indio_dev, NULL,
- bmi160_trigger_handler, NULL);
+ ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
+ bmi160_trigger_handler, NULL);
if (ret < 0)
- goto uninit;
+ return ret;
- ret = iio_device_register(indio_dev);
+ ret = devm_iio_device_register(dev, indio_dev);
if (ret < 0)
- goto buffer_cleanup;
+ return ret;
return 0;
-buffer_cleanup:
- iio_triggered_buffer_cleanup(indio_dev);
-uninit:
- bmi160_chip_uninit(data);
- return ret;
}
EXPORT_SYMBOL_GPL(bmi160_core_probe);
-void bmi160_core_remove(struct device *dev)
-{
- struct iio_dev *indio_dev = dev_get_drvdata(dev);
- struct bmi160_data *data = iio_priv(indio_dev);
-
- iio_device_unregister(indio_dev);
- iio_triggered_buffer_cleanup(indio_dev);
- bmi160_chip_uninit(data);
-}
-EXPORT_SYMBOL_GPL(bmi160_core_remove);
-
MODULE_AUTHOR("Daniel Baluta <daniel.baluta@intel.com");
MODULE_DESCRIPTION("Bosch BMI160 driver");
MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/imu/bmi160/bmi160_i2c.c b/drivers/iio/imu/bmi160/bmi160_i2c.c
index 155a31f72445..5b1f7e6af651 100644
--- a/drivers/iio/imu/bmi160/bmi160_i2c.c
+++ b/drivers/iio/imu/bmi160/bmi160_i2c.c
@@ -38,13 +38,6 @@ static int bmi160_i2c_probe(struct i2c_client *client,
return bmi160_core_probe(&client->dev, regmap, name, false);
}
-static int bmi160_i2c_remove(struct i2c_client *client)
-{
- bmi160_core_remove(&client->dev);
-
- return 0;
-}
-
static const struct i2c_device_id bmi160_i2c_id[] = {
{"bmi160", 0},
{}
@@ -72,7 +65,6 @@ static struct i2c_driver bmi160_i2c_driver = {
.of_match_table = of_match_ptr(bmi160_of_match),
},
.probe = bmi160_i2c_probe,
- .remove = bmi160_i2c_remove,
.id_table = bmi160_i2c_id,
};
module_i2c_driver(bmi160_i2c_driver);
diff --git a/drivers/iio/imu/bmi160/bmi160_spi.c b/drivers/iio/imu/bmi160/bmi160_spi.c
index d34dfdfd1a7d..e521ad14eeac 100644
--- a/drivers/iio/imu/bmi160/bmi160_spi.c
+++ b/drivers/iio/imu/bmi160/bmi160_spi.c
@@ -29,13 +29,6 @@ static int bmi160_spi_probe(struct spi_device *spi)
return bmi160_core_probe(&spi->dev, regmap, id->name, true);
}
-static int bmi160_spi_remove(struct spi_device *spi)
-{
- bmi160_core_remove(&spi->dev);
-
- return 0;
-}
-
static const struct spi_device_id bmi160_spi_id[] = {
{"bmi160", 0},
{}
@@ -58,7 +51,6 @@ MODULE_DEVICE_TABLE(of, bmi160_of_match);
static struct spi_driver bmi160_spi_driver = {
.probe = bmi160_spi_probe,
- .remove = bmi160_spi_remove,
.id_table = bmi160_spi_id,
.driver = {
.acpi_match_table = ACPI_PTR(bmi160_acpi_match),
diff --git a/drivers/iio/imu/st_lsm6dsx/Makefile b/drivers/iio/imu/st_lsm6dsx/Makefile
index 35919febea2a..e5f733ce6e11 100644
--- a/drivers/iio/imu/st_lsm6dsx/Makefile
+++ b/drivers/iio/imu/st_lsm6dsx/Makefile
@@ -1,4 +1,5 @@
-st_lsm6dsx-y := st_lsm6dsx_core.o st_lsm6dsx_buffer.o
+st_lsm6dsx-y := st_lsm6dsx_core.o st_lsm6dsx_buffer.o \
+ st_lsm6dsx_shub.o
obj-$(CONFIG_IIO_ST_LSM6DSX) += st_lsm6dsx.o
obj-$(CONFIG_IIO_ST_LSM6DSX_I2C) += st_lsm6dsx_i2c.o
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index ef73519a0fb6..d1d8d07a0714 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -43,6 +43,24 @@ enum st_lsm6dsx_hw_id {
* ST_LSM6DSX_TAGGED_SAMPLE_SIZE)
#define ST_LSM6DSX_SHIFT_VAL(val, mask) (((val) << __ffs(mask)) & (mask))
+#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, \
+ }, \
+}
+
struct st_lsm6dsx_reg {
u8 addr;
u8 mask;
@@ -50,6 +68,28 @@ struct st_lsm6dsx_reg {
struct st_lsm6dsx_hw;
+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];
+};
+
+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];
+};
+
/**
* struct st_lsm6dsx_fifo_ops - ST IMU FIFO settings
* @read_fifo: Read FIFO callback.
@@ -85,6 +125,70 @@ struct st_lsm6dsx_hw_ts_settings {
};
/**
+ * 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).
+ * @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.
+ */
+struct st_lsm6dsx_shub_settings {
+ struct st_lsm6dsx_reg page_mux;
+ struct st_lsm6dsx_reg master_en;
+ struct st_lsm6dsx_reg pullup_en;
+ struct st_lsm6dsx_reg aux_sens;
+ struct st_lsm6dsx_reg wr_once;
+ u8 shub_out;
+ u8 slv0_addr;
+ u8 dw_slv0_addr;
+ u8 batch_en;
+};
+
+enum st_lsm6dsx_ext_sensor_id {
+ ST_LSM6DSX_ID_MAGN,
+};
+
+/**
+ * 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: Output data rate of the sensor [Hz].
+ * @gain: Configured sensor sensitivity.
+ * @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
* @wai: Sensor WhoAmI default value.
* @max_fifo_size: Sensor max fifo length in FIFO words.
@@ -93,6 +197,7 @@ struct st_lsm6dsx_hw_ts_settings {
* @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 {
u8 wai;
@@ -102,11 +207,15 @@ struct st_lsm6dsx_settings {
struct st_lsm6dsx_reg batch[ST_LSM6DSX_MAX_ID];
struct st_lsm6dsx_fifo_ops fifo_ops;
struct st_lsm6dsx_hw_ts_settings ts_settings;
+ struct st_lsm6dsx_shub_settings shub_settings;
};
enum st_lsm6dsx_sensor_id {
- ST_LSM6DSX_ID_ACC,
ST_LSM6DSX_ID_GYRO,
+ ST_LSM6DSX_ID_ACC,
+ ST_LSM6DSX_ID_EXT0,
+ ST_LSM6DSX_ID_EXT1,
+ ST_LSM6DSX_ID_EXT2,
ST_LSM6DSX_ID_MAX,
};
@@ -126,6 +235,7 @@ enum st_lsm6dsx_fifo_mode {
* @sip: Number of samples in a given pattern.
* @decimator: FIFO decimation factor.
* @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];
@@ -139,6 +249,11 @@ struct st_lsm6dsx_sensor {
u8 sip;
u8 decimator;
s64 ts_ref;
+
+ struct {
+ const struct st_lsm6dsx_ext_dev_settings *settings;
+ u8 addr;
+ } ext_info;
};
/**
@@ -148,6 +263,7 @@ struct st_lsm6dsx_sensor {
* @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.
* @fifo_mode: FIFO operating mode supported by the device.
* @enable_mask: Enabled sensor bitmask.
* @ts_sip: Total number of timestamp samples in a given pattern.
@@ -163,6 +279,7 @@ struct st_lsm6dsx_hw {
struct mutex fifo_lock;
struct mutex conf_lock;
+ struct mutex page_lock;
enum st_lsm6dsx_fifo_mode fifo_mode;
u8 enable_mask;
@@ -176,13 +293,15 @@ struct st_lsm6dsx_hw {
const struct st_lsm6dsx_settings *settings;
};
+static 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, const char *name,
struct regmap *regmap);
-int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor);
-int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor);
+int st_lsm6dsx_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_flush_fifo(struct st_lsm6dsx_hw *hw);
@@ -191,5 +310,47 @@ int st_lsm6dsx_set_fifo_mode(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, u16 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_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;
+}
#endif /* ST_LSM6DSX_H */
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index b5263fc522ca..2c0d3763405a 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -68,6 +68,9 @@ 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,
};
static const
@@ -102,6 +105,9 @@ static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
*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)))
@@ -125,6 +131,9 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
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)) {
@@ -142,8 +151,9 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
if (dec_reg->addr) {
int val = ST_LSM6DSX_SHIFT_VAL(data, dec_reg->mask);
- err = regmap_update_bits(hw->regmap, dec_reg->addr,
- dec_reg->mask, val);
+ err = st_lsm6dsx_update_bits_locked(hw, dec_reg->addr,
+ dec_reg->mask,
+ val);
if (err < 0)
return err;
}
@@ -162,8 +172,8 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
int val, ts_dec = !!hw->ts_sip;
val = ST_LSM6DSX_SHIFT_VAL(ts_dec, ts_dec_reg->mask);
- err = regmap_update_bits(hw->regmap, ts_dec_reg->addr,
- ts_dec_reg->mask, val);
+ err = st_lsm6dsx_update_bits_locked(hw, ts_dec_reg->addr,
+ ts_dec_reg->mask, val);
}
return err;
}
@@ -171,12 +181,12 @@ static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
enum st_lsm6dsx_fifo_mode fifo_mode)
{
+ unsigned int data;
int err;
- err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
- ST_LSM6DSX_FIFO_MODE_MASK,
- FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK,
- fifo_mode));
+ data = FIELD_PREP(ST_LSM6DSX_FIFO_MODE_MASK, fifo_mode);
+ err = st_lsm6dsx_update_bits_locked(hw, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
+ ST_LSM6DSX_FIFO_MODE_MASK, data);
if (err < 0)
return err;
@@ -207,15 +217,15 @@ static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor,
data = 0;
}
val = ST_LSM6DSX_SHIFT_VAL(data, batch_reg->mask);
- return regmap_update_bits(hw->regmap, batch_reg->addr,
- batch_reg->mask, val);
+ 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 regmap_update_bits(hw->regmap,
- ST_LSM6DSX_REG_FIFO_MODE_ADDR,
- ST_LSM6DSX_FIFO_ODR_MASK,
- FIELD_PREP(ST_LSM6DSX_FIFO_ODR_MASK,
- data));
+ 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));
}
}
@@ -231,6 +241,9 @@ int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
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)))
@@ -246,19 +259,23 @@ int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
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)
- return err;
+ 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);
- return regmap_bulk_write(hw->regmap,
- hw->settings->fifo_ops.fifo_th.addr,
- &wdata, sizeof(wdata));
+ 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)
@@ -267,12 +284,15 @@ static int st_lsm6dsx_reset_hw_ts(struct st_lsm6dsx_hw *hw)
int i, err;
/* reset hw ts counter */
- err = regmap_write(hw->regmap, ST_LSM6DSX_REG_TS_RESET_ADDR,
- ST_LSM6DSX_TS_RESET_VAL);
+ 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
@@ -297,8 +317,8 @@ static inline int st_lsm6dsx_read_block(struct st_lsm6dsx_hw *hw, u8 addr,
while (read_len < data_len) {
word_len = min_t(unsigned int, data_len - read_len,
max_word_len);
- err = regmap_bulk_read(hw->regmap, addr, data + read_len,
- word_len);
+ err = st_lsm6dsx_read_locked(hw, addr, data + read_len,
+ word_len);
if (err < 0)
return err;
read_len += word_len;
@@ -328,9 +348,9 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
__le16 fifo_status;
s64 ts = 0;
- err = regmap_bulk_read(hw->regmap,
- hw->settings->fifo_ops.fifo_diff.addr,
- &fifo_status, sizeof(fifo_status));
+ 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);
@@ -436,6 +456,55 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
return read_len;
}
+static int
+st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
+ u8 *data, s64 ts)
+{
+ struct st_lsm6dsx_sensor *sensor;
+ struct iio_dev *iio_dev;
+
+ /*
+ * 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;
+ 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() - LSM6DSO read FIFO routine
* @hw: Pointer to instance of struct st_lsm6dsx_hw.
@@ -455,9 +524,9 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
__le16 fifo_status;
s64 ts = 0;
- err = regmap_bulk_read(hw->regmap,
- hw->settings->fifo_ops.fifo_diff.addr,
- &fifo_status, sizeof(fifo_status));
+ 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);
@@ -491,8 +560,7 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
ST_LSM6DSX_SAMPLE_SIZE);
tag = hw->buff[i] >> 3;
- switch (tag) {
- case ST_LSM6DSX_TS_TAG:
+ if (tag == ST_LSM6DSX_TS_TAG) {
/*
* hw timestamp is 4B long and it is stored
* in FIFO according to this schema:
@@ -509,19 +577,9 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw)
if (!reset_ts && ts >= 0xffff0000)
reset_ts = true;
ts *= ST_LSM6DSX_TS_SENSITIVITY;
- break;
- case ST_LSM6DSX_GYRO_TAG:
- iio_push_to_buffers_with_timestamp(
- hw->iio_devs[ST_LSM6DSX_ID_GYRO],
- iio_buff, gyro_sensor->ts_ref + ts);
- break;
- case ST_LSM6DSX_ACC_TAG:
- iio_push_to_buffers_with_timestamp(
- hw->iio_devs[ST_LSM6DSX_ID_ACC],
- iio_buff, acc_sensor->ts_ref + ts);
- break;
- default:
- break;
+ } else {
+ st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,
+ ts);
}
}
}
@@ -562,19 +620,21 @@ static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
goto out;
}
- if (enable) {
- err = st_lsm6dsx_sensor_enable(sensor);
+ if (sensor->id == ST_LSM6DSX_ID_EXT0 ||
+ sensor->id == ST_LSM6DSX_ID_EXT1 ||
+ sensor->id == ST_LSM6DSX_ID_EXT2) {
+ err = st_lsm6dsx_shub_set_enable(sensor, enable);
if (err < 0)
goto out;
} else {
- err = st_lsm6dsx_sensor_disable(sensor);
+ err = st_lsm6dsx_sensor_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_set_fifo_odr(sensor, enable);
+ if (err < 0)
+ goto out;
+ }
err = st_lsm6dsx_update_decimators(hw);
if (err < 0)
@@ -690,6 +750,9 @@ int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw)
}
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+ if (!hw->iio_devs[i])
+ continue;
+
buffer = devm_iio_kfifo_allocate(hw->dev);
if (!buffer)
return -ENOMEM;
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 2ad3c610e4b6..12e29dda9b98 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -56,6 +56,7 @@
#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_BOOT_MASK BIT(7)
#define ST_LSM6DSX_REG_BDU_ADDR 0x12
#define ST_LSM6DSX_REG_BDU_MASK BIT(6)
#define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR 0x13
@@ -87,17 +88,6 @@
#define ST_LSM6DSX_GYRO_FS_1000_GAIN IIO_DEGREE_TO_RAD(35000)
#define ST_LSM6DSX_GYRO_FS_2000_GAIN IIO_DEGREE_TO_RAD(70000)
-struct st_lsm6dsx_odr {
- u16 hz;
- u8 val;
-};
-
-#define ST_LSM6DSX_ODR_LIST_SIZE 6
-struct st_lsm6dsx_odr_table_entry {
- struct st_lsm6dsx_reg reg;
- struct st_lsm6dsx_odr odr_avl[ST_LSM6DSX_ODR_LIST_SIZE];
-};
-
static const struct st_lsm6dsx_odr_table_entry st_lsm6dsx_odr_table[] = {
[ST_LSM6DSX_ID_ACC] = {
.reg = {
@@ -125,17 +115,6 @@ static const struct st_lsm6dsx_odr_table_entry st_lsm6dsx_odr_table[] = {
}
};
-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 = {
@@ -341,27 +320,35 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
.mask = GENMASK(7, 6),
},
},
+ .shub_settings = {
+ .page_mux = {
+ .addr = 0x01,
+ .mask = BIT(6),
+ },
+ .master_en = {
+ .addr = 0x14,
+ .mask = BIT(2),
+ },
+ .pullup_en = {
+ .addr = 0x14,
+ .mask = BIT(3),
+ },
+ .aux_sens = {
+ .addr = 0x14,
+ .mask = GENMASK(1, 0),
+ },
+ .wr_once = {
+ .addr = 0x14,
+ .mask = BIT(6),
+ },
+ .shub_out = 0x02,
+ .slv0_addr = 0x15,
+ .dw_slv0_addr = 0x21,
+ .batch_en = BIT(3),
+ }
},
};
-#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \
-{ \
- .type = chan_type, \
- .address = addr, \
- .modified = 1, \
- .channel2 = mod, \
- .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
- BIT(IIO_CHAN_INFO_SCALE), \
- .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
- .scan_index = scan_idx, \
- .scan_type = { \
- .sign = 's', \
- .realbits = 16, \
- .storagebits = 16, \
- .endianness = IIO_LE, \
- }, \
-}
-
static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR,
IIO_MOD_X, 0),
@@ -382,6 +369,21 @@ static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = {
IIO_CHAN_SOFT_TIMESTAMP(3),
};
+int st_lsm6dsx_set_page(struct st_lsm6dsx_hw *hw, bool enable)
+{
+ const struct st_lsm6dsx_shub_settings *hub_settings;
+ unsigned int data;
+ int err;
+
+ hub_settings = &hw->settings->shub_settings;
+ data = ST_LSM6DSX_SHIFT_VAL(enable, hub_settings->page_mux.mask);
+ err = regmap_update_bits(hw->regmap, hub_settings->page_mux.addr,
+ hub_settings->page_mux.mask, data);
+ usleep_range(100, 150);
+
+ return err;
+}
+
static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id)
{
int err, i, j, data;
@@ -421,6 +423,7 @@ static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor,
{
struct st_lsm6dsx_hw *hw = sensor->hw;
const struct st_lsm6dsx_reg *reg;
+ unsigned int data;
int i, err;
u8 val;
@@ -433,8 +436,8 @@ static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor,
val = st_lsm6dsx_fs_table[sensor->id].fs_avl[i].val;
reg = &st_lsm6dsx_fs_table[sensor->id].reg;
- err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
- ST_LSM6DSX_SHIFT_VAL(val, reg->mask));
+ data = ST_LSM6DSX_SHIFT_VAL(val, reg->mask);
+ err = st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask, data);
if (err < 0)
return err;
@@ -448,7 +451,11 @@ int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr, u8 *val)
int i;
for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
- if (st_lsm6dsx_odr_table[sensor->id].odr_avl[i].hz == odr)
+ /*
+ * ext devices can run at different odr respect to
+ * accel sensor
+ */
+ if (st_lsm6dsx_odr_table[sensor->id].odr_avl[i].hz >= odr)
break;
if (i == ST_LSM6DSX_ODR_LIST_SIZE)
@@ -459,48 +466,86 @@ int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr, u8 *val)
return 0;
}
-static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
+static u16 st_lsm6dsx_check_odr_dependency(struct st_lsm6dsx_hw *hw, u16 odr,
+ enum st_lsm6dsx_sensor_id id)
{
- struct st_lsm6dsx_hw *hw = sensor->hw;
- const struct st_lsm6dsx_reg *reg;
- int err;
- u8 val;
-
- err = st_lsm6dsx_check_odr(sensor, odr, &val);
- if (err < 0)
- return err;
-
- reg = &st_lsm6dsx_odr_table[sensor->id].reg;
- return regmap_update_bits(hw->regmap, reg->addr, reg->mask,
- ST_LSM6DSX_SHIFT_VAL(val, reg->mask));
+ struct st_lsm6dsx_sensor *ref = iio_priv(hw->iio_devs[id]);
+
+ if (odr > 0) {
+ if (hw->enable_mask & BIT(id))
+ return max_t(u16, ref->odr, odr);
+ else
+ return odr;
+ } else {
+ return (hw->enable_mask & BIT(id)) ? ref->odr : 0;
+ }
}
-int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor)
+static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 req_odr)
{
+ struct st_lsm6dsx_sensor *ref_sensor = sensor;
+ struct st_lsm6dsx_hw *hw = sensor->hw;
+ const struct st_lsm6dsx_reg *reg;
+ unsigned int data;
+ u8 val = 0;
int err;
- err = st_lsm6dsx_set_odr(sensor, sensor->odr);
- if (err < 0)
- return err;
+ switch (sensor->id) {
+ case ST_LSM6DSX_ID_EXT0:
+ case ST_LSM6DSX_ID_EXT1:
+ case ST_LSM6DSX_ID_EXT2:
+ case ST_LSM6DSX_ID_ACC: {
+ u16 odr;
+ int i;
+
+ /*
+ * i2c embedded controller relies on the accelerometer sensor as
+ * bus read/write trigger so we need to enable accel device
+ * at odr = max(accel_odr, ext_odr) in order to properly
+ * communicate with i2c slave devices
+ */
+ ref_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
+ for (i = ST_LSM6DSX_ID_ACC; i < ST_LSM6DSX_ID_MAX; i++) {
+ if (!hw->iio_devs[i] || i == sensor->id)
+ continue;
+
+ odr = st_lsm6dsx_check_odr_dependency(hw, req_odr, i);
+ if (odr != req_odr)
+ /* device already configured */
+ return 0;
+ }
+ break;
+ }
+ default:
+ break;
+ }
- sensor->hw->enable_mask |= BIT(sensor->id);
+ if (req_odr > 0) {
+ err = st_lsm6dsx_check_odr(ref_sensor, req_odr, &val);
+ if (err < 0)
+ return err;
+ }
- return 0;
+ reg = &st_lsm6dsx_odr_table[ref_sensor->id].reg;
+ data = ST_LSM6DSX_SHIFT_VAL(val, reg->mask);
+ return st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask, data);
}
-int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor)
+int st_lsm6dsx_sensor_set_enable(struct st_lsm6dsx_sensor *sensor,
+ bool enable)
{
struct st_lsm6dsx_hw *hw = sensor->hw;
- const struct st_lsm6dsx_reg *reg;
+ u16 odr = enable ? sensor->odr : 0;
int err;
- reg = &st_lsm6dsx_odr_table[sensor->id].reg;
- err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
- ST_LSM6DSX_SHIFT_VAL(0, reg->mask));
+ err = st_lsm6dsx_set_odr(sensor, odr);
if (err < 0)
return err;
- sensor->hw->enable_mask &= ~BIT(sensor->id);
+ if (enable)
+ hw->enable_mask |= BIT(sensor->id);
+ else
+ hw->enable_mask &= ~BIT(sensor->id);
return 0;
}
@@ -512,18 +557,18 @@ static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
int err, delay;
__le16 data;
- err = st_lsm6dsx_sensor_enable(sensor);
+ err = st_lsm6dsx_sensor_set_enable(sensor, true);
if (err < 0)
return err;
delay = 1000000 / sensor->odr;
usleep_range(delay, 2 * delay);
- err = regmap_bulk_read(hw->regmap, addr, &data, sizeof(data));
+ err = st_lsm6dsx_read_locked(hw, addr, &data, sizeof(data));
if (err < 0)
return err;
- st_lsm6dsx_sensor_disable(sensor);
+ st_lsm6dsx_sensor_set_enable(sensor, false);
*val = (s16)le16_to_cpu(data);
@@ -596,7 +641,7 @@ static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev,
return err;
}
-static int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val)
+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;
@@ -692,8 +737,6 @@ static const struct iio_info st_lsm6dsx_gyro_info = {
.hwfifo_set_watermark = st_lsm6dsx_set_watermark,
};
-static const unsigned long st_lsm6dsx_available_scan_masks[] = {0x7, 0x0};
-
static int st_lsm6dsx_of_get_drdy_pin(struct st_lsm6dsx_hw *hw, int *drdy_pin)
{
struct device_node *np = hw->dev->of_node;
@@ -732,6 +775,51 @@ static int st_lsm6dsx_get_drdy_reg(struct st_lsm6dsx_hw *hw, u8 *drdy_reg)
return err;
}
+static int st_lsm6dsx_init_shub(struct st_lsm6dsx_hw *hw)
+{
+ const struct st_lsm6dsx_shub_settings *hub_settings;
+ struct device_node *np = hw->dev->of_node;
+ struct st_sensors_platform_data *pdata;
+ unsigned int data;
+ int err = 0;
+
+ hub_settings = &hw->settings->shub_settings;
+
+ pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
+ if ((np && of_property_read_bool(np, "st,pullups")) ||
+ (pdata && pdata->pullups)) {
+ err = st_lsm6dsx_set_page(hw, true);
+ if (err < 0)
+ return err;
+
+ data = ST_LSM6DSX_SHIFT_VAL(1, hub_settings->pullup_en.mask);
+ err = regmap_update_bits(hw->regmap,
+ hub_settings->pullup_en.addr,
+ hub_settings->pullup_en.mask, data);
+
+ st_lsm6dsx_set_page(hw, false);
+
+ if (err < 0)
+ return err;
+ }
+
+ if (hub_settings->aux_sens.addr) {
+ /* configure aux sensors */
+ err = st_lsm6dsx_set_page(hw, true);
+ if (err < 0)
+ return err;
+
+ data = ST_LSM6DSX_SHIFT_VAL(3, hub_settings->aux_sens.mask);
+ err = regmap_update_bits(hw->regmap,
+ hub_settings->aux_sens.addr,
+ hub_settings->aux_sens.mask, data);
+
+ st_lsm6dsx_set_page(hw, false);
+ }
+
+ return err;
+}
+
static int st_lsm6dsx_init_hw_timer(struct st_lsm6dsx_hw *hw)
{
const struct st_lsm6dsx_hw_ts_settings *ts_settings;
@@ -775,12 +863,23 @@ static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
u8 drdy_int_reg;
int err;
- err = regmap_write(hw->regmap, ST_LSM6DSX_REG_RESET_ADDR,
- ST_LSM6DSX_REG_RESET_MASK);
+ /* device sw reset */
+ err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_RESET_ADDR,
+ ST_LSM6DSX_REG_RESET_MASK,
+ FIELD_PREP(ST_LSM6DSX_REG_RESET_MASK, 1));
+ if (err < 0)
+ return err;
+
+ msleep(50);
+
+ /* reload trimming parameter */
+ err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_RESET_ADDR,
+ ST_LSM6DSX_REG_BOOT_MASK,
+ FIELD_PREP(ST_LSM6DSX_REG_BOOT_MASK, 1));
if (err < 0)
return err;
- msleep(200);
+ msleep(50);
/* enable Block Data Update */
err = regmap_update_bits(hw->regmap, ST_LSM6DSX_REG_BDU_ADDR,
@@ -801,6 +900,10 @@ static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
if (err < 0)
return err;
+ err = st_lsm6dsx_init_shub(hw);
+ if (err < 0)
+ return err;
+
return st_lsm6dsx_init_hw_timer(hw);
}
@@ -854,6 +957,7 @@ static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
struct regmap *regmap)
{
+ const struct st_lsm6dsx_shub_settings *hub_settings;
struct st_lsm6dsx_hw *hw;
int i, err;
@@ -865,6 +969,7 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
mutex_init(&hw->fifo_lock);
mutex_init(&hw->conf_lock);
+ mutex_init(&hw->page_lock);
hw->buff = devm_kzalloc(dev, ST_LSM6DSX_BUFF_SIZE, GFP_KERNEL);
if (!hw->buff)
@@ -878,7 +983,7 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
if (err < 0)
return err;
- for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+ for (i = 0; i < ST_LSM6DSX_ID_EXT0; i++) {
hw->iio_devs[i] = st_lsm6dsx_alloc_iiodev(hw, i, name);
if (!hw->iio_devs[i])
return -ENOMEM;
@@ -888,6 +993,13 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
if (err < 0)
return err;
+ hub_settings = &hw->settings->shub_settings;
+ if (hub_settings->master_en.addr) {
+ err = st_lsm6dsx_shub_probe(hw, name);
+ if (err < 0)
+ return err;
+ }
+
if (hw->irq > 0) {
err = st_lsm6dsx_fifo_setup(hw);
if (err < 0)
@@ -895,6 +1007,9 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
}
for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
+ if (!hw->iio_devs[i])
+ continue;
+
err = devm_iio_device_register(hw->dev, hw->iio_devs[i]);
if (err)
return err;
@@ -909,16 +1024,21 @@ static int __maybe_unused st_lsm6dsx_suspend(struct device *dev)
struct st_lsm6dsx_hw *hw = dev_get_drvdata(dev);
struct st_lsm6dsx_sensor *sensor;
const struct st_lsm6dsx_reg *reg;
+ unsigned int data;
int i, err = 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;
reg = &st_lsm6dsx_odr_table[sensor->id].reg;
- err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
- ST_LSM6DSX_SHIFT_VAL(0, reg->mask));
+ data = ST_LSM6DSX_SHIFT_VAL(0, reg->mask);
+ err = st_lsm6dsx_update_bits_locked(hw, reg->addr, reg->mask,
+ data);
if (err < 0)
return err;
}
@@ -936,6 +1056,9 @@ static int __maybe_unused st_lsm6dsx_resume(struct device *dev)
int i, err = 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;
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
new file mode 100644
index 000000000000..8e47dccdd40f
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
@@ -0,0 +1,779 @@
+/*
+ * 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_MAX_SLV_NUM 3
+#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] = { 10, 0x0 },
+ .odr_avl[1] = { 20, 0x1 },
+ .odr_avl[2] = { 50, 0x2 },
+ .odr_avl[3] = { 100, 0x3 },
+ },
+ .fs_table = {
+ .fs_avl[0] = {
+ .gain = 1500,
+ .val = 0x0,
+ }, /* 1500 uG/LSB */
+ },
+ .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,
+ },
+ },
+};
+
+static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw)
+{
+ struct st_lsm6dsx_sensor *sensor;
+
+ sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
+ msleep((2000U / sensor->odr) + 1);
+}
+
+/**
+ * st_lsm6dsx_shub_read_reg - read i2c controller register
+ *
+ * Read st_lsm6dsx i2c controller register
+ */
+static int st_lsm6dsx_shub_read_reg(struct st_lsm6dsx_hw *hw, u8 addr,
+ 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;
+ err = st_lsm6dsx_set_page(hw, true);
+ if (err < 0)
+ goto out;
+
+ err = regmap_bulk_read(hw->regmap, addr, data, len);
+
+ 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;
+ 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);
+
+ 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;
+ struct st_lsm6dsx_hw *hw = sensor->hw;
+ u8 config[3], slv_addr;
+ int err;
+
+ hub_settings = &hw->settings->shub_settings;
+ slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
+
+ config[0] = (sensor->ext_info.addr << 1) | 1;
+ config[1] = addr;
+ config[2] = len & ST_LS6DSX_READ_OP_MASK;
+
+ 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_reg(hw, hub_settings->shub_out, data,
+ len & ST_LS6DSX_READ_OP_MASK);
+
+ st_lsm6dsx_shub_master_enable(sensor, false);
+
+ memset(config, 0, sizeof(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);
+ }
+
+ memset(config, 0, sizeof(config));
+ 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,
+ u16 odr, u16 *val)
+{
+ const struct st_lsm6dsx_ext_dev_settings *settings;
+ int i;
+
+ settings = sensor->ext_info.settings;
+ for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
+ if (settings->odr_table.odr_avl[i].hz == odr)
+ break;
+
+ if (i == ST_LSM6DSX_ODR_LIST_SIZE)
+ return -EINVAL;
+
+ *val = settings->odr_table.odr_avl[i].val;
+ return 0;
+}
+
+static int
+st_lsm6dsx_shub_set_odr(struct st_lsm6dsx_sensor *sensor, u16 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->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 = 1000000 / sensor->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);
+
+ 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:
+ ret = iio_device_claim_direct_mode(iio_dev);
+ if (ret)
+ break;
+
+ ret = st_lsm6dsx_shub_read_oneshot(sensor, ch, 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_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;
+
+ err = iio_device_claim_direct_mode(iio_dev);
+ if (err)
+ return err;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SAMP_FREQ: {
+ u16 data;
+
+ err = st_lsm6dsx_shub_get_odr_val(sensor, val, &data);
+ if (!err)
+ sensor->odr = val;
+ break;
+ }
+ default:
+ err = -EINVAL;
+ break;
+ }
+
+ iio_device_release_direct_mode(iio_dev);
+
+ return err;
+}
+
+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 < ST_LSM6DSX_ODR_LIST_SIZE; i++) {
+ u16 val = settings->odr_table.odr_avl[i].hz;
+
+ if (val > 0)
+ len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
+ val);
+ }
+ 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 < ST_LSM6DSX_FS_LIST_SIZE; i++) {
+ u16 val = settings->fs_table.fs_avl[i].gain;
+
+ if (val > 0)
+ len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
+ val);
+ }
+ 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_ext_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_ext_attribute_group = {
+ .attrs = st_lsm6dsx_ext_attributes,
+};
+
+static const struct iio_info st_lsm6dsx_ext_info = {
+ .attrs = &st_lsm6dsx_ext_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)
+{
+ 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->dev.parent = hw->dev;
+ iio_dev->info = &st_lsm6dsx_ext_info;
+
+ sensor = iio_priv(iio_dev);
+ sensor->id = id;
+ sensor->hw = hw;
+ sensor->odr = info->odr_table.odr_avl[0].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;
+ struct st_lsm6dsx_sensor *sensor;
+ u8 config[3], data, slv_addr;
+ bool found = false;
+ int i, err;
+
+ hub_settings = &hw->settings->shub_settings;
+ slv_addr = ST_LSM6DSX_SLV_ADDR(0, hub_settings->slv0_addr);
+ sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
+
+ 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;
+
+ 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_reg(hw,
+ hub_settings->shub_out,
+ &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 */
+ memset(config, 0, sizeof(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 >= ST_LSM6DSX_MAX_SLV_NUM)
+ break;
+ id++;
+ }
+
+ return 0;
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-06 07:01:15 +0000