soc: imx: add i.MX8M feature controller driver

The tester4 fuse bank of the i.MX8M is a 32-bit collection of fuses,
apaprently fused during test, which contains information about the
available IPs: How many cores are available and whether a VPU and GPU
is available an usable. Add a imx8m_feat_ctrl_init() function
that initializes a bitmap of supported features using tester4's value
and registers a feature controller with a check callback that just
looks up the relevant bit.

This function can then be called from a standalone driver or from the
fuse bank (ocotp) driver itself.

Signed-off-by: Ahmad Fatoum <a.fatoum@pengutronix.de>
Link: https://lore.barebox.org/20220818051955.2088238-6-a.fatoum@pengutronix.de
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

3 files changed, 71 insertions, 0 deletions

diff --git a/drivers/soc/imx/Kconfig b/drivers/soc/imx/Kconfig
index 742d13c4f5..eabe4a06d2 100644
--- a/drivers/soc/imx/Kconfig
+++ b/drivers/soc/imx/Kconfig
@@ -7,4 +7,10 @@ config IMX_GPCV2_PM_DOMAINS
 	select PM_GENERIC_DOMAINS
 	default y if ARCH_IMX7 || ARCH_IMX8MQ
 
+config IMX8M_FEATCTRL
+        bool "i.MX8M feature controller"
+	depends on ARCH_IMX8M
+	select FEATURE_CONTROLLER
+	default y
+
 endmenu
diff --git a/drivers/soc/imx/Makefile b/drivers/soc/imx/Makefile
index 3a8a8d0b00..bd1717b038 100644
--- a/drivers/soc/imx/Makefile
+++ b/drivers/soc/imx/Makefile
@@ -1,2 +1,3 @@
 # SPDX-License-Identifier: GPL-2.0-only
 obj-$(CONFIG_IMX_GPCV2_PM_DOMAINS) += gpcv2.o
+obj-$(CONFIG_IMX8M_FEATCTRL) += imx8m-featctrl.o
diff --git a/drivers/soc/imx/imx8m-featctrl.c b/drivers/soc/imx/imx8m-featctrl.c
new file mode 100644
index 0000000000..480c80e6c1
--- /dev/null
+++ b/drivers/soc/imx/imx8m-featctrl.c
@@ -0,0 +1,64 @@
+// SPDX-License-Identifier: GPL-2.0-only
+// SPDX-FileCopyrightText: 2022 Ahmad Fatoum, Pengutronix
+
+#include <common.h>
+#include <linux/bitmap.h>
+#include <featctrl.h>
+#include <soc/imx8m/featctrl.h>
+
+#include <dt-bindings/features/imx8m.h>
+
+struct imx_feat {
+	struct feature_controller feat;
+	unsigned long features[BITS_TO_LONGS(IMX8M_FEAT_END)];
+};
+
+static struct imx_feat *to_imx_feat(struct feature_controller *feat)
+{
+	return container_of(feat, struct imx_feat, feat);
+}
+
+static int imx8m_feat_check(struct feature_controller *feat, int idx)
+{
+	struct imx_feat *priv = to_imx_feat(feat);
+
+	if (idx > IMX8M_FEAT_END)
+		return -EINVAL;
+
+	return test_bit(idx, priv->features) ? FEATCTRL_OKAY : FEATCTRL_GATED;
+}
+
+int imx8m_feat_ctrl_init(struct device_d *dev, u32 tester4,
+			 const struct imx8m_featctrl_data *data)
+{
+	unsigned long *features;
+	struct imx_feat *priv;
+
+	if (!dev || !data)
+		return -ENODEV;
+
+	dev_dbg(dev, "tester4 = 0x%08x\n", tester4);
+
+	priv = xzalloc(sizeof(*priv));
+	features = priv->features;
+
+	bitmap_fill(features, IMX8M_FEAT_END);
+
+	if (tester4 & data->vpu_bitmask)
+		clear_bit(IMX8M_FEAT_VPU, features);
+	if (tester4 & data->gpu_bitmask)
+		clear_bit(IMX8M_FEAT_GPU, features);
+
+	switch (tester4 & 3) {
+	case 0b11:
+		clear_bit(IMX8M_FEAT_CPU_DUAL, features);
+		fallthrough;
+	case 0b10:
+		clear_bit(IMX8M_FEAT_CPU_QUAD, features);
+	}
+
+	priv->feat.dev = dev;
+	priv->feat.check = imx8m_feat_check;
+
+	return feature_controller_register(&priv->feat);
+}