[PATCH V2 2/5] iio: imu: inv_icm42600: Add support for using alternate registers

[Chris Morgan <macroalpha82@gmail.com>]

From: Chris Morgan <macromorgan@hotmail.com>

Add support to the existing inv_icm42600 to support similar hardware
with slightly different register layouts. For example the icm42607
and icm42607p has most of the same functionality and even many of
the same registers, but the addresses for indiviual registers differ.

Signed-off-by: Chris Morgan <macromorgan@hotmail.com>
---
 drivers/iio/imu/inv_icm42600/inv_icm42600.h   |  28 +-
 .../iio/imu/inv_icm42600/inv_icm42600_accel.c |  15 +-
 .../iio/imu/inv_icm42600/inv_icm42600_core.c  | 257 ++++++++++--------
 .../iio/imu/inv_icm42600/inv_icm42600_gyro.c  |  13 +-
 4 files changed, 188 insertions(+), 125 deletions(-)

diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
index c8b48a5c5ed0..b89078cb5ba0 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -143,6 +143,26 @@ struct inv_icm42600_apex {
 	} wom;
 };
 
+typedef int (*inv_icm42600_bus_setup)(struct inv_icm42600_state *);
+
+struct inv_icm42600_funcs {
+	int (*setup)(struct inv_icm42600_state *st,
+		     inv_icm42600_bus_setup bus_setup);
+
+	/* timestamp_setup optional, not present on icm42607 */
+	int (*timestamp_setup)(struct inv_icm42600_state *st);
+
+	int (*buffer_init)(struct inv_icm42600_state *st);
+	int (*gyro_init)(struct inv_icm42600_state *st,
+			 struct iio_dev *indio_dev);
+	int (*accel_init)(struct inv_icm42600_state *st,
+			  struct iio_dev *indio_dev);
+	int (*suspend)(struct device *dev);
+	int (*resume)(struct device *dev);
+	int (*runtime_suspend)(struct device *dev);
+	int (*runtime_resume)(struct device *dev);
+};
+
 /**
  *  struct inv_icm42600_state - driver state variables
  *  @lock:		lock for serializing multiple registers access.
@@ -160,6 +180,7 @@ struct inv_icm42600_apex {
  *  @timestamp:		interrupt timestamps.
  *  @apex:		APEX (Advanced Pedometer and Event detection) management
  *  @fifo:		FIFO management structure.
+ *  @hw_funcs:		Device specific hardware functions.
  *  @buffer:		data transfer buffer aligned for DMA.
  */
 struct inv_icm42600_state {
@@ -180,6 +201,7 @@ struct inv_icm42600_state {
 	} timestamp;
 	struct inv_icm42600_apex apex;
 	struct inv_icm42600_fifo fifo;
+	struct inv_icm42600_funcs *hw_funcs;
 	u8 buffer[3] __aligned(IIO_DMA_MINALIGN);
 };
 
@@ -440,8 +462,6 @@ struct inv_icm42600_sensor_state {
 #define INV_ICM42600_TEMP_STARTUP_TIME_MS	14
 #define INV_ICM42600_SUSPEND_DELAY_MS		2000
 
-typedef int (*inv_icm42600_bus_setup)(struct inv_icm42600_state *);
-
 extern const struct regmap_config inv_icm42600_regmap_config;
 extern const struct regmap_config inv_icm42600_spi_regmap_config;
 extern const struct dev_pm_ops inv_icm42600_pm_ops;
@@ -472,11 +492,11 @@ int inv_icm42600_debugfs_reg(struct iio_dev *indio_dev, unsigned int reg,
 int inv_icm42600_core_probe(struct regmap *regmap, int chip,
 			    inv_icm42600_bus_setup bus_setup);
 
-struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st);
+int inv_icm42600_gyro_init(struct inv_icm42600_state *st, struct iio_dev *indio_dev);
 
 int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev);
 
-struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st);
+int inv_icm42600_accel_init(struct inv_icm42600_state *st, struct iio_dev *indio_dev);
 
 int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev);
 
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
index 0ab6eddf0543..cbb27c4e6b82 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -1137,22 +1137,21 @@ static const struct iio_info inv_icm42600_accel_info = {
 	.write_event_value = inv_icm42600_accel_write_event_value,
 };
 
-struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
+int inv_icm42600_accel_init(struct inv_icm42600_state *st, struct iio_dev *indio_dev)
 {
 	struct device *dev = regmap_get_device(st->map);
 	const char *name;
 	struct inv_icm42600_sensor_state *accel_st;
 	struct inv_sensors_timestamp_chip ts_chip;
-	struct iio_dev *indio_dev;
 	int ret;
 
 	name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
 	if (!name)
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
 
 	indio_dev = devm_iio_device_alloc(dev, sizeof(*accel_st));
 	if (!indio_dev)
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
 	accel_st = iio_priv(indio_dev);
 
 	switch (st->chip) {
@@ -1189,18 +1188,18 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
 	ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
 					  &inv_icm42600_buffer_ops);
 	if (ret)
-		return ERR_PTR(ret);
+		return ret;
 
 	ret = devm_iio_device_register(dev, indio_dev);
 	if (ret)
-		return ERR_PTR(ret);
+		return ret;
 
 	/* accel events are wakeup capable */
 	ret = devm_device_init_wakeup(&indio_dev->dev);
 	if (ret)
-		return ERR_PTR(ret);
+		return ret;
 
-	return indio_dev;
+	return 0;
 }
 
 int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 76eb22488e5f..29c8c1871e06 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -703,113 +703,11 @@ static void inv_icm42600_disable_vddio_reg(void *_data)
 	regulator_disable(st->vddio_supply);
 }
 
-int inv_icm42600_core_probe(struct regmap *regmap, int chip,
-			    inv_icm42600_bus_setup bus_setup)
-{
-	struct device *dev = regmap_get_device(regmap);
-	struct fwnode_handle *fwnode = dev_fwnode(dev);
-	struct inv_icm42600_state *st;
-	int irq, irq_type;
-	bool open_drain;
-	int ret;
-
-	if (chip <= INV_CHIP_INVALID || chip >= INV_CHIP_NB) {
-		dev_err(dev, "invalid chip = %d\n", chip);
-		return -ENODEV;
-	}
-
-	/* get INT1 only supported interrupt or fallback to first interrupt */
-	irq = fwnode_irq_get_byname(fwnode, "INT1");
-	if (irq < 0 && irq != -EPROBE_DEFER) {
-		dev_info(dev, "no INT1 interrupt defined, fallback to first interrupt\n");
-		irq = fwnode_irq_get(fwnode, 0);
-	}
-	if (irq < 0)
-		return dev_err_probe(dev, irq, "error missing INT1 interrupt\n");
-
-	irq_type = irq_get_trigger_type(irq);
-	if (!irq_type)
-		irq_type = IRQF_TRIGGER_FALLING;
-
-	open_drain = device_property_read_bool(dev, "drive-open-drain");
-
-	st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL);
-	if (!st)
-		return -ENOMEM;
-
-	dev_set_drvdata(dev, st);
-	mutex_init(&st->lock);
-	st->chip = chip;
-	st->map = regmap;
-	st->irq = irq;
-
-	ret = iio_read_mount_matrix(dev, &st->orientation);
-	if (ret) {
-		dev_err(dev, "failed to retrieve mounting matrix %d\n", ret);
-		return ret;
-	}
-
-	ret = devm_regulator_get_enable(dev, "vdd");
-	if (ret)
-		return dev_err_probe(dev, ret,
-				     "Failed to get vdd regulator\n");
-
-	msleep(INV_ICM42600_POWER_UP_TIME_MS);
-
-	st->vddio_supply = devm_regulator_get(dev, "vddio");
-	if (IS_ERR(st->vddio_supply))
-		return PTR_ERR(st->vddio_supply);
-
-	ret = inv_icm42600_enable_regulator_vddio(st);
-	if (ret)
-		return ret;
-
-	ret = devm_add_action_or_reset(dev, inv_icm42600_disable_vddio_reg, st);
-	if (ret)
-		return ret;
-
-	/* setup chip registers */
-	ret = inv_icm42600_setup(st, bus_setup);
-	if (ret)
-		return ret;
-
-	ret = inv_icm42600_timestamp_setup(st);
-	if (ret)
-		return ret;
-
-	ret = inv_icm42600_buffer_init(st);
-	if (ret)
-		return ret;
-
-	st->indio_gyro = inv_icm42600_gyro_init(st);
-	if (IS_ERR(st->indio_gyro))
-		return PTR_ERR(st->indio_gyro);
-
-	st->indio_accel = inv_icm42600_accel_init(st);
-	if (IS_ERR(st->indio_accel))
-		return PTR_ERR(st->indio_accel);
-
-	ret = inv_icm42600_irq_init(st, irq, irq_type, open_drain);
-	if (ret)
-		return ret;
-
-	/* setup runtime power management */
-	ret = devm_pm_runtime_set_active_enabled(dev);
-	if (ret)
-		return ret;
-
-	pm_runtime_set_autosuspend_delay(dev, INV_ICM42600_SUSPEND_DELAY_MS);
-	pm_runtime_use_autosuspend(dev);
-
-	return ret;
-}
-EXPORT_SYMBOL_NS_GPL(inv_icm42600_core_probe, "IIO_ICM42600");
-
 /*
  * Suspend saves sensors state and turns everything off.
  * Check first if runtime suspend has not already done the job.
  */
-static int inv_icm42600_suspend(struct device *dev)
+static int inv_icm42600_hw_suspend(struct device *dev)
 {
 	struct inv_icm42600_state *st = dev_get_drvdata(dev);
 	struct device *accel_dev;
@@ -867,7 +765,7 @@ static int inv_icm42600_suspend(struct device *dev)
  * System resume gets the system back on and restores the sensors state.
  * Manually put runtime power management in system active state.
  */
-static int inv_icm42600_resume(struct device *dev)
+static int inv_icm42600_hw_resume(struct device *dev)
 {
 	struct inv_icm42600_state *st = dev_get_drvdata(dev);
 	struct inv_icm42600_sensor_state *gyro_st = iio_priv(st->indio_gyro);
@@ -920,7 +818,7 @@ static int inv_icm42600_resume(struct device *dev)
 }
 
 /* Runtime suspend will turn off sensors that are enabled by iio devices. */
-static int inv_icm42600_runtime_suspend(struct device *dev)
+static int inv_icm42600_hw_runtime_suspend(struct device *dev)
 {
 	struct inv_icm42600_state *st = dev_get_drvdata(dev);
 	int ret;
@@ -940,7 +838,7 @@ static int inv_icm42600_runtime_suspend(struct device *dev)
 }
 
 /* Sensors are enabled by iio devices, no need to turn them back on here. */
-static int inv_icm42600_runtime_resume(struct device *dev)
+static int inv_icm42600_hw_runtime_resume(struct device *dev)
 {
 	struct inv_icm42600_state *st = dev_get_drvdata(dev);
 
@@ -949,6 +847,153 @@ static int inv_icm42600_runtime_resume(struct device *dev)
 	return inv_icm42600_enable_regulator_vddio(st);
 }
 
+static struct inv_icm42600_funcs inv_icm42600_hw_funcs = {
+	.setup = &inv_icm42600_setup,
+	.timestamp_setup = &inv_icm42600_timestamp_setup,
+	.buffer_init = &inv_icm42600_buffer_init,
+	.gyro_init = &inv_icm42600_gyro_init,
+	.accel_init = &inv_icm42600_accel_init,
+	.suspend = &inv_icm42600_hw_suspend,
+	.resume = &inv_icm42600_hw_resume,
+	.runtime_suspend = &inv_icm42600_hw_runtime_suspend,
+	.runtime_resume = &inv_icm42600_hw_runtime_resume,
+};
+
+int inv_icm42600_core_probe(struct regmap *regmap, int chip,
+			    inv_icm42600_bus_setup bus_setup)
+{
+	struct device *dev = regmap_get_device(regmap);
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct inv_icm42600_state *st;
+	int irq, irq_type;
+	bool open_drain;
+	int ret;
+
+	if (chip <= INV_CHIP_INVALID || chip >= INV_CHIP_NB) {
+		dev_err(dev, "invalid chip = %d\n", chip);
+		return -ENODEV;
+	}
+
+	/* get INT1 only supported interrupt or fallback to first interrupt */
+	irq = fwnode_irq_get_byname(fwnode, "INT1");
+	if (irq < 0 && irq != -EPROBE_DEFER) {
+		dev_info(dev, "no INT1 interrupt defined, fallback to first interrupt\n");
+		irq = fwnode_irq_get(fwnode, 0);
+	}
+	if (irq < 0)
+		return dev_err_probe(dev, irq, "error missing INT1 interrupt\n");
+
+	irq_type = irq_get_trigger_type(irq);
+	if (!irq_type)
+		irq_type = IRQF_TRIGGER_FALLING;
+
+	open_drain = device_property_read_bool(dev, "drive-open-drain");
+
+	st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL);
+	if (!st)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, st);
+	mutex_init(&st->lock);
+	st->chip = chip;
+	st->map = regmap;
+	st->irq = irq;
+
+	ret = iio_read_mount_matrix(dev, &st->orientation);
+	if (ret) {
+		dev_err(dev, "failed to retrieve mounting matrix %d\n", ret);
+		return ret;
+	}
+
+	ret = devm_regulator_get_enable(dev, "vdd");
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "Failed to get vdd regulator\n");
+
+	msleep(INV_ICM42600_POWER_UP_TIME_MS);
+
+	st->vddio_supply = devm_regulator_get(dev, "vddio");
+	if (IS_ERR(st->vddio_supply))
+		return PTR_ERR(st->vddio_supply);
+
+	ret = inv_icm42600_enable_regulator_vddio(st);
+	if (ret)
+		return ret;
+
+	ret = devm_add_action_or_reset(dev, inv_icm42600_disable_vddio_reg, st);
+	if (ret)
+		return ret;
+
+	/* setup chip registers based on hardware */
+	st->hw_funcs = &inv_icm42600_hw_funcs;
+
+	ret = st->hw_funcs->setup(st, bus_setup);
+	if (ret)
+		return ret;
+
+	/* Timestamp setup optional for ICM42607 */
+	if (st->hw_funcs->timestamp_setup) {
+		ret = st->hw_funcs->timestamp_setup(st);
+		if (ret)
+			return ret;
+	}
+
+	ret = st->hw_funcs->buffer_init(st);
+	if (ret)
+		return ret;
+
+	ret = st->hw_funcs->gyro_init(st, st->indio_gyro);
+	if (ret)
+		return ret;
+
+	ret = st->hw_funcs->accel_init(st, st->indio_accel);
+	if (ret)
+		return ret;
+
+	ret = inv_icm42600_irq_init(st, irq, irq_type, open_drain);
+	if (ret)
+		return ret;
+
+	/* setup runtime power management */
+	ret = devm_pm_runtime_set_active_enabled(dev);
+	if (ret)
+		return ret;
+
+	pm_runtime_set_autosuspend_delay(dev, INV_ICM42600_SUSPEND_DELAY_MS);
+	pm_runtime_use_autosuspend(dev);
+
+	return ret;
+}
+EXPORT_SYMBOL_NS_GPL(inv_icm42600_core_probe, "IIO_ICM42600");
+
+static int inv_icm42600_suspend(struct device *dev)
+{
+	struct inv_icm42600_state *st = dev_get_drvdata(dev);
+
+	return st->hw_funcs->suspend(dev);
+}
+
+static int inv_icm42600_resume(struct device *dev)
+{
+	struct inv_icm42600_state *st = dev_get_drvdata(dev);
+
+	return st->hw_funcs->resume(dev);
+}
+
+static int inv_icm42600_runtime_suspend(struct device *dev)
+{
+	struct inv_icm42600_state *st = dev_get_drvdata(dev);
+
+	return st->hw_funcs->runtime_suspend(dev);
+}
+
+static int inv_icm42600_runtime_resume(struct device *dev)
+{
+	struct inv_icm42600_state *st = dev_get_drvdata(dev);
+
+	return st->hw_funcs->runtime_resume(dev);
+}
+
 EXPORT_NS_GPL_DEV_PM_OPS(inv_icm42600_pm_ops, IIO_ICM42600) = {
 	SYSTEM_SLEEP_PM_OPS(inv_icm42600_suspend, inv_icm42600_resume)
 	RUNTIME_PM_OPS(inv_icm42600_runtime_suspend,
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
index 11339ddf1da3..32aa2e52df2e 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -725,22 +725,21 @@ static const struct iio_info inv_icm42600_gyro_info = {
 	.hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush,
 };
 
-struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
+int inv_icm42600_gyro_init(struct inv_icm42600_state *st, struct iio_dev *indio_dev)
 {
 	struct device *dev = regmap_get_device(st->map);
 	const char *name;
 	struct inv_icm42600_sensor_state *gyro_st;
 	struct inv_sensors_timestamp_chip ts_chip;
-	struct iio_dev *indio_dev;
 	int ret;
 
 	name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
 	if (!name)
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
 
 	indio_dev = devm_iio_device_alloc(dev, sizeof(*gyro_st));
 	if (!indio_dev)
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
 	gyro_st = iio_priv(indio_dev);
 
 	switch (st->chip) {
@@ -775,13 +774,13 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
 	ret = devm_iio_kfifo_buffer_setup(dev, indio_dev,
 					  &inv_icm42600_buffer_ops);
 	if (ret)
-		return ERR_PTR(ret);
+		return ret;
 
 	ret = devm_iio_device_register(dev, indio_dev);
 	if (ret)
-		return ERR_PTR(ret);
+		return ret;
 
-	return indio_dev;
+	return 0;
 }
 
 int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
-- 
2.43.0