1 files changed, 98 insertions, 31 deletions

diff --git a/drivers/iio/adc/stm32-dfsdm-core.c b/drivers/iio/adc/stm32-dfsdm-core.c
index a627af9a825e..47e2d1338e9e 100644
--- a/drivers/iio/adc/stm32-dfsdm-core.c
+++ b/drivers/iio/adc/stm32-dfsdm-core.c
@@ -6,20 +6,32 @@
  * Author(s): Arnaud Pouliquen <arnaud.pouliquen@st.com> for STMicroelectronics.
  */
 
+#include <linux/bitfield.h>
 #include <linux/clk.h>
+#include <linux/export.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
-#include <linux/of_device.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
 #include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/regmap.h>
 #include <linux/slab.h>
 
 #include "stm32-dfsdm.h"
 
+/**
+ * struct stm32_dfsdm_dev_data - DFSDM compatible configuration data
+ * @ipid: DFSDM identification number. Used only if hardware provides identification registers
+ * @num_filters: DFSDM number of filters. Unused if identification registers are available
+ * @num_channels: DFSDM number of channels. Unused if identification registers are available
+ * @regmap_cfg: SAI register map configuration pointer
+ */
 struct stm32_dfsdm_dev_data {
+	u32 ipid;
 	unsigned int num_filters;
 	unsigned int num_channels;
 	const struct regmap_config *regmap_cfg;
@@ -27,8 +39,6 @@ struct stm32_dfsdm_dev_data {
 
 #define STM32H7_DFSDM_NUM_FILTERS	4
 #define STM32H7_DFSDM_NUM_CHANNELS	8
-#define STM32MP1_DFSDM_NUM_FILTERS	6
-#define STM32MP1_DFSDM_NUM_CHANNELS	8
 
 static bool stm32_dfsdm_volatile_reg(struct device *dev, unsigned int reg)
 {
@@ -75,8 +85,7 @@ static const struct regmap_config stm32mp1_dfsdm_regmap_cfg = {
 };
 
 static const struct stm32_dfsdm_dev_data stm32mp1_dfsdm_data = {
-	.num_filters = STM32MP1_DFSDM_NUM_FILTERS,
-	.num_channels = STM32MP1_DFSDM_NUM_CHANNELS,
+	.ipid = STM32MP15_IPIDR_NUMBER,
 	.regmap_cfg = &stm32mp1_dfsdm_regmap_cfg,
 };
 
@@ -291,10 +300,69 @@ static const struct of_device_id stm32_dfsdm_of_match[] = {
 		.compatible = "st,stm32mp1-dfsdm",
 		.data = &stm32mp1_dfsdm_data,
 	},
-	{}
+	{ }
 };
 MODULE_DEVICE_TABLE(of, stm32_dfsdm_of_match);
 
+static int stm32_dfsdm_probe_identification(struct platform_device *pdev,
+					    struct dfsdm_priv *priv,
+					    const struct stm32_dfsdm_dev_data *dev_data)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *child;
+	struct stm32_dfsdm *dfsdm = &priv->dfsdm;
+	const char *compat;
+	int ret, count = 0;
+	u32 id, val;
+
+	if (!dev_data->ipid) {
+		dfsdm->num_fls = dev_data->num_filters;
+		dfsdm->num_chs = dev_data->num_channels;
+		return 0;
+	}
+
+	ret = regmap_read(dfsdm->regmap, DFSDM_IPIDR, &id);
+	if (ret)
+		return ret;
+
+	if (id != dev_data->ipid) {
+		dev_err(&pdev->dev, "Unexpected IP version: 0x%x", id);
+		return -EINVAL;
+	}
+
+	for_each_child_of_node(np, child) {
+		ret = of_property_read_string(child, "compatible", &compat);
+		if (ret)
+			continue;
+		/* Count only child nodes with dfsdm compatible */
+		if (strstr(compat, "dfsdm"))
+			count++;
+	}
+
+	ret = regmap_read(dfsdm->regmap, DFSDM_HWCFGR, &val);
+	if (ret)
+		return ret;
+
+	dfsdm->num_fls = FIELD_GET(DFSDM_HWCFGR_NBF_MASK, val);
+	dfsdm->num_chs = FIELD_GET(DFSDM_HWCFGR_NBT_MASK, val);
+
+	if (count > dfsdm->num_fls) {
+		dev_err(&pdev->dev, "Unexpected child number: %d", count);
+		return -EINVAL;
+	}
+
+	ret = regmap_read(dfsdm->regmap, DFSDM_VERR, &val);
+	if (ret)
+		return ret;
+
+	dev_dbg(&pdev->dev, "DFSDM version: %lu.%lu. %d channels/%d filters\n",
+		FIELD_GET(DFSDM_VERR_MAJREV_MASK, val),
+		FIELD_GET(DFSDM_VERR_MINREV_MASK, val),
+		dfsdm->num_chs, dfsdm->num_fls);
+
+	return 0;
+}
+
 static int stm32_dfsdm_probe(struct platform_device *pdev)
 {
 	struct dfsdm_priv *priv;
@@ -311,18 +379,6 @@ static int stm32_dfsdm_probe(struct platform_device *pdev)
 	dev_data = of_device_get_match_data(&pdev->dev);
 
 	dfsdm = &priv->dfsdm;
-	dfsdm->fl_list = devm_kcalloc(&pdev->dev, dev_data->num_filters,
-				      sizeof(*dfsdm->fl_list), GFP_KERNEL);
-	if (!dfsdm->fl_list)
-		return -ENOMEM;
-
-	dfsdm->num_fls = dev_data->num_filters;
-	dfsdm->ch_list = devm_kcalloc(&pdev->dev, dev_data->num_channels,
-				      sizeof(*dfsdm->ch_list),
-				      GFP_KERNEL);
-	if (!dfsdm->ch_list)
-		return -ENOMEM;
-	dfsdm->num_chs = dev_data->num_channels;
 
 	ret = stm32_dfsdm_parse_of(pdev, priv);
 	if (ret < 0)
@@ -338,6 +394,20 @@ static int stm32_dfsdm_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	ret = stm32_dfsdm_probe_identification(pdev, priv, dev_data);
+	if (ret < 0)
+		return ret;
+
+	dfsdm->fl_list = devm_kcalloc(&pdev->dev, dfsdm->num_fls,
+				      sizeof(*dfsdm->fl_list), GFP_KERNEL);
+	if (!dfsdm->fl_list)
+		return -ENOMEM;
+
+	dfsdm->ch_list = devm_kcalloc(&pdev->dev, dfsdm->num_chs,
+				      sizeof(*dfsdm->ch_list), GFP_KERNEL);
+	if (!dfsdm->ch_list)
+		return -ENOMEM;
+
 	platform_set_drvdata(pdev, dfsdm);
 
 	ret = stm32_dfsdm_clk_prepare_enable(dfsdm);
@@ -367,7 +437,7 @@ pm_put:
 	return ret;
 }
 
-static int stm32_dfsdm_core_remove(struct platform_device *pdev)
+static void stm32_dfsdm_core_remove(struct platform_device *pdev)
 {
 	struct stm32_dfsdm *dfsdm = platform_get_drvdata(pdev);
 
@@ -377,11 +447,9 @@ static int stm32_dfsdm_core_remove(struct platform_device *pdev)
 	pm_runtime_set_suspended(&pdev->dev);
 	pm_runtime_put_noidle(&pdev->dev);
 	stm32_dfsdm_clk_disable_unprepare(dfsdm);
-
-	return 0;
 }
 
-static int __maybe_unused stm32_dfsdm_core_suspend(struct device *dev)
+static int stm32_dfsdm_core_suspend(struct device *dev)
 {
 	struct stm32_dfsdm *dfsdm = dev_get_drvdata(dev);
 	struct dfsdm_priv *priv = to_stm32_dfsdm_priv(dfsdm);
@@ -397,7 +465,7 @@ static int __maybe_unused stm32_dfsdm_core_suspend(struct device *dev)
 	return pinctrl_pm_select_sleep_state(dev);
 }
 
-static int __maybe_unused stm32_dfsdm_core_resume(struct device *dev)
+static int stm32_dfsdm_core_resume(struct device *dev)
 {
 	struct stm32_dfsdm *dfsdm = dev_get_drvdata(dev);
 	struct dfsdm_priv *priv = to_stm32_dfsdm_priv(dfsdm);
@@ -414,7 +482,7 @@ static int __maybe_unused stm32_dfsdm_core_resume(struct device *dev)
 	return pm_runtime_force_resume(dev);
 }
 
-static int __maybe_unused stm32_dfsdm_core_runtime_suspend(struct device *dev)
+static int stm32_dfsdm_core_runtime_suspend(struct device *dev)
 {
 	struct stm32_dfsdm *dfsdm = dev_get_drvdata(dev);
 
@@ -423,7 +491,7 @@ static int __maybe_unused stm32_dfsdm_core_runtime_suspend(struct device *dev)
 	return 0;
 }
 
-static int __maybe_unused stm32_dfsdm_core_runtime_resume(struct device *dev)
+static int stm32_dfsdm_core_runtime_resume(struct device *dev)
 {
 	struct stm32_dfsdm *dfsdm = dev_get_drvdata(dev);
 
@@ -431,11 +499,10 @@ static int __maybe_unused stm32_dfsdm_core_runtime_resume(struct device *dev)
 }
 
 static const struct dev_pm_ops stm32_dfsdm_core_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(stm32_dfsdm_core_suspend,
-				stm32_dfsdm_core_resume)
-	SET_RUNTIME_PM_OPS(stm32_dfsdm_core_runtime_suspend,
-			   stm32_dfsdm_core_runtime_resume,
-			   NULL)
+	SYSTEM_SLEEP_PM_OPS(stm32_dfsdm_core_suspend, stm32_dfsdm_core_resume)
+	RUNTIME_PM_OPS(stm32_dfsdm_core_runtime_suspend,
+		       stm32_dfsdm_core_runtime_resume,
+		       NULL)
 };
 
 static struct platform_driver stm32_dfsdm_driver = {
@@ -444,7 +511,7 @@ static struct platform_driver stm32_dfsdm_driver = {
 	.driver = {
 		.name = "stm32-dfsdm",
 		.of_match_table = stm32_dfsdm_of_match,
-		.pm = &stm32_dfsdm_core_pm_ops,
+		.pm = pm_ptr(&stm32_dfsdm_core_pm_ops),
 	},
 };