1 files changed, 203 insertions, 80 deletions

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;