diff options
Diffstat (limited to 'drivers/soc/imx/imx8m-featctrl.c')
-rw-r--r-- | drivers/soc/imx/imx8m-featctrl.c | 90 |
1 files changed, 90 insertions, 0 deletions
diff --git a/drivers/soc/imx/imx8m-featctrl.c b/drivers/soc/imx/imx8m-featctrl.c new file mode 100644 index 0000000000..31579aff7e --- /dev/null +++ b/drivers/soc/imx/imx8m-featctrl.c @@ -0,0 +1,90 @@ +// 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; +} + +static inline bool is_fused(u32 val, u32 bitmask) +{ + return bitmask && (val & bitmask) == bitmask; +} + +#define field_get(_mask, _reg) (((_reg) & (_mask)) >> (ffs(_mask) - 1)) + +static void check_cpus(u32 mask, u32 reg, unsigned long *features) +{ + switch (field_get(mask, reg)) { + case 0b11: + clear_bit(IMX8M_FEAT_CPU_DUAL, features); + fallthrough; + case 0b10: + clear_bit(IMX8M_FEAT_CPU_QUAD, features); + } +} + +int imx8m_feat_ctrl_init(struct device *dev, u32 tester3, u32 tester4, + const struct imx8m_featctrl_data *data) +{ + unsigned long *features; + struct imx_feat *priv; + + if (!dev || !data) + return -ENODEV; + + dev_dbg(dev, "tester3 = 0x%08x, tester4 = 0x%08x\n", tester3, tester4); + + priv = xzalloc(sizeof(*priv)); + features = priv->features; + + bitmap_fill(features, IMX8M_FEAT_END); + + if (is_fused(tester3, data->tester3.vpu_bitmask) || + is_fused(tester4, data->tester4.vpu_bitmask)) + clear_bit(IMX8M_FEAT_VPU, features); + if (is_fused(tester4, data->tester4.gpu_bitmask)) + clear_bit(IMX8M_FEAT_GPU, features); + if (is_fused(tester4, data->tester4.mipi_dsi_bitmask)) + clear_bit(IMX8M_FEAT_MIPI_DSI, features); + if (is_fused(tester4, data->tester4.isp_bitmask)) + clear_bit(IMX8M_FEAT_ISP, features); + if (is_fused(tester4, data->tester4.npu_bitmask)) + clear_bit(IMX8M_FEAT_NPU, features); + if (is_fused(tester4, data->tester4.lvds_bitmask)) + clear_bit(IMX8M_FEAT_LVDS, features); + if (is_fused(tester4, data->tester4.dsp_bitmask)) + clear_bit(IMX8M_FEAT_DSP, features); + + if (data->tester3.cpu_bitmask) + check_cpus(data->tester3.cpu_bitmask, tester3, features); + else if (data->tester4.cpu_bitmask) + check_cpus(data->tester4.cpu_bitmask, tester4, features); + + priv->feat.dev = dev; + priv->feat.check = imx8m_feat_check; + + return feature_controller_register(&priv->feat); +} |