[PATCH 4/7] phy: freescale: add imx93 MIPI CSI2 DPHY support

@@ -44,6 +44,16 @@ config PHY_FSL_IMX8QM_HSIO
 	  Enable this to add support for the HSIO PHY as found on
 	  i.MX8QM family of SOCs.
 
+config PHY_FSL_IMX93_DPHY_RX
+	tristate "Freescale i.MX9 DPHY Rx"
+	depends on OF && HAS_IOMEM
+	select GENERIC_PHY
+	select GENERIC_PHY_MIPI_DPHY
+	select REGMAP_MMIO
+	help
+	  Enable this to add support for the Synopsys DW DPHY Rx as found
+	  on NXP's i.MX9 family.
+
 config PHY_FSL_SAMSUNG_HDMI_PHY
 	tristate "Samsung HDMI PHY support"
 	depends on OF && HAS_IOMEM && COMMON_CLK

@@ -4,5 +4,6 @@ obj-$(CONFIG_PHY_MIXEL_LVDS_PHY)	+= phy-fsl-imx8qm-lvds-phy.o
 obj-$(CONFIG_PHY_MIXEL_MIPI_DPHY)	+= phy-fsl-imx8-mipi-dphy.o
 obj-$(CONFIG_PHY_FSL_IMX8M_PCIE)	+= phy-fsl-imx8m-pcie.o
 obj-$(CONFIG_PHY_FSL_IMX8QM_HSIO)	+= phy-fsl-imx8qm-hsio.o
+obj-$(CONFIG_PHY_FSL_IMX93_DPHY_RX)	+= phy-fsl-imx93-dphy-rx.o
 obj-$(CONFIG_PHY_FSL_LYNX_28G)		+= phy-fsl-lynx-28g.o
 obj-$(CONFIG_PHY_FSL_SAMSUNG_HDMI_PHY)	+= phy-fsl-samsung-hdmi.o

@@ -0,0 +1,306 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright 2025 NXP
+ */
+
+#include <linux/bitfield.h>
+#include <linux/bits.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/phy/phy.h>
+#include <linux/phy/phy-mipi-dphy.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#define IMX93_BLK_CSI			0x48
+#define   IMX93_BLK_CSI_CFGCLKFREQRANGE	GENMASK(5, 0)
+#define   IMX93_BLK_CSI_HSFREQRANGE	GENMASK(14, 8)
+
+struct fsl_csi2_phy_drv_data {
+	u32 max_lanes;
+	u32 max_data_rate; /* Mbps */
+};
+
+struct fsl_csi2_phy {
+	struct device *dev;
+	struct regmap *dphy_regmap;
+	struct clk *cfg_clk;
+
+	const struct fsl_csi2_phy_drv_data *drv_data;
+
+	u16 hsfreqrange;
+	u16 cfgclkfreqrange;
+	u16 ddlfreq;
+};
+
+struct dphy_mbps_hsfreqrange_map {
+	u16 mbps;
+	u16 hsfreqrange;
+	u16 ddlfreq;
+};
+
+/*
+ * Data rate to high speed frequency range map table
+ */
+static const struct dphy_mbps_hsfreqrange_map hsfreqrange_table[] = {
+	{ .mbps = 80,  .hsfreqrange = 0x00, .ddlfreq = 489 },
+	{ .mbps = 90,  .hsfreqrange = 0x10, .ddlfreq = 489 },
+	{ .mbps = 100, .hsfreqrange = 0x20, .ddlfreq = 489 },
+	{ .mbps = 110, .hsfreqrange = 0x30, .ddlfreq = 489 },
+	{ .mbps = 120, .hsfreqrange = 0x01, .ddlfreq = 489 },
+	{ .mbps = 130, .hsfreqrange = 0x11, .ddlfreq = 489 },
+	{ .mbps = 140, .hsfreqrange = 0x21, .ddlfreq = 489 },
+	{ .mbps = 150, .hsfreqrange = 0x31, .ddlfreq = 489 },
+	{ .mbps = 160, .hsfreqrange = 0x02, .ddlfreq = 489 },
+	{ .mbps = 170, .hsfreqrange = 0x12, .ddlfreq = 489 },
+	{ .mbps = 180, .hsfreqrange = 0x22, .ddlfreq = 489 },
+	{ .mbps = 190, .hsfreqrange = 0x32, .ddlfreq = 489 },
+	{ .mbps = 205, .hsfreqrange = 0x03, .ddlfreq = 489 },
+	{ .mbps = 220, .hsfreqrange = 0x13, .ddlfreq = 489 },
+	{ .mbps = 235, .hsfreqrange = 0x23, .ddlfreq = 489 },
+	{ .mbps = 250, .hsfreqrange = 0x33, .ddlfreq = 489 },
+	{ .mbps = 275, .hsfreqrange = 0x04, .ddlfreq = 489 },
+	{ .mbps = 300, .hsfreqrange = 0x14, .ddlfreq = 489 },
+	{ .mbps = 325, .hsfreqrange = 0x25, .ddlfreq = 489 },
+	{ .mbps = 350, .hsfreqrange = 0x35, .ddlfreq = 489 },
+	{ .mbps = 400, .hsfreqrange = 0x05, .ddlfreq = 489 },
+	{ .mbps = 450, .hsfreqrange = 0x16, .ddlfreq = 489 },
+	{ .mbps = 500, .hsfreqrange = 0x26, .ddlfreq = 489 },
+	{ .mbps = 550, .hsfreqrange = 0x37, .ddlfreq = 489 },
+	{ .mbps = 600, .hsfreqrange = 0x07, .ddlfreq = 489 },
+	{ .mbps = 650, .hsfreqrange = 0x18, .ddlfreq = 489 },
+	{ .mbps = 700, .hsfreqrange = 0x28, .ddlfreq = 489 },
+	{ .mbps = 750, .hsfreqrange = 0x39, .ddlfreq = 489 },
+	{ .mbps = 800, .hsfreqrange = 0x09, .ddlfreq = 489 },
+	{ .mbps = 850, .hsfreqrange = 0x19, .ddlfreq = 489 },
+	{ .mbps = 900, .hsfreqrange = 0x29, .ddlfreq = 489 },
+	{ .mbps = 950, .hsfreqrange = 0x3a, .ddlfreq = 489 },
+	{ .mbps = 1000, .hsfreqrange = 0x0a, .ddlfreq = 489 },
+	{ .mbps = 1050, .hsfreqrange = 0x1a, .ddlfreq = 489 },
+	{ .mbps = 1100, .hsfreqrange = 0x2a, .ddlfreq = 489 },
+	{ .mbps = 1150, .hsfreqrange = 0x3b, .ddlfreq = 489 },
+	{ .mbps = 1200, .hsfreqrange = 0x0b, .ddlfreq = 489 },
+	{ .mbps = 1250, .hsfreqrange = 0x1b, .ddlfreq = 489 },
+	{ .mbps = 1300, .hsfreqrange = 0x2b, .ddlfreq = 489 },
+	{ .mbps = 1350, .hsfreqrange = 0x3c, .ddlfreq = 489 },
+	{ .mbps = 1400, .hsfreqrange = 0x0c, .ddlfreq = 489 },
+	{ .mbps = 1450, .hsfreqrange = 0x1c, .ddlfreq = 489 },
+	{ .mbps = 1500, .hsfreqrange = 0x2c, .ddlfreq = 489 },
+	{ .mbps = 1550, .hsfreqrange = 0x3d, .ddlfreq = 303 },
+	{ .mbps = 1600, .hsfreqrange = 0x0d, .ddlfreq = 313 },
+	{ .mbps = 1650, .hsfreqrange = 0x1d, .ddlfreq = 323 },
+	{ .mbps = 1700, .hsfreqrange = 0x2e, .ddlfreq = 333 },
+	{ .mbps = 1750, .hsfreqrange = 0x3e, .ddlfreq = 342 },
+	{ .mbps = 1800, .hsfreqrange = 0x0e, .ddlfreq = 352 },
+	{ .mbps = 1850, .hsfreqrange = 0x1e, .ddlfreq = 362 },
+	{ .mbps = 1900, .hsfreqrange = 0x1f, .ddlfreq = 372 },
+	{ .mbps = 1950, .hsfreqrange = 0x3f, .ddlfreq = 381 },
+	{ .mbps = 2000, .hsfreqrange = 0x0f, .ddlfreq = 391 },
+	{ .mbps = 2050, .hsfreqrange = 0x40, .ddlfreq = 401 },
+	{ .mbps = 2100, .hsfreqrange = 0x41, .ddlfreq = 411 },
+	{ .mbps = 2150, .hsfreqrange = 0x42, .ddlfreq = 411 },
+	{ .mbps = 2200, .hsfreqrange = 0x43, .ddlfreq = 411 },
+	{ .mbps = 2250, .hsfreqrange = 0x44, .ddlfreq = 411 },
+	{ .mbps = 2300, .hsfreqrange = 0x45, .ddlfreq = 411 },
+	{ .mbps = 2350, .hsfreqrange = 0x46, .ddlfreq = 411 },
+	{ .mbps = 2400, .hsfreqrange = 0x47, .ddlfreq = 411 },
+	{ .mbps = 2450, .hsfreqrange = 0x48, .ddlfreq = 411 },
+	{ .mbps = 2500, .hsfreqrange = 0x49, .ddlfreq = 411 },
+	{ /* sentinel */ },
+};
+
+static int fsl_csi2_phy_init(struct phy *phy)
+{
+	struct fsl_csi2_phy *priv = phy_get_drvdata(phy);
+
+	return pm_runtime_get_sync(priv->dev);
+}
+
+static int fsl_csi2_phy_exit(struct phy *phy)
+{
+	struct fsl_csi2_phy *priv = phy_get_drvdata(phy);
+
+	return pm_runtime_put(priv->dev);
+}
+
+static int fsl_csi2_phy_power_on(struct phy *phy)
+{
+	struct fsl_csi2_phy *priv = phy_get_drvdata(phy);
+
+	regmap_update_bits(priv->dphy_regmap, IMX93_BLK_CSI,
+			   IMX93_BLK_CSI_CFGCLKFREQRANGE,
+			   FIELD_PREP(IMX93_BLK_CSI_CFGCLKFREQRANGE, priv->cfgclkfreqrange));
+
+	regmap_update_bits(priv->dphy_regmap, IMX93_BLK_CSI,
+			   IMX93_BLK_CSI_HSFREQRANGE,
+			   FIELD_PREP(IMX93_BLK_CSI_HSFREQRANGE, priv->hsfreqrange));
+
+	return 0;
+}
+
+static int set_freqrange_by_mpbs(struct fsl_csi2_phy *priv, u64 mbps)
+{
+	const struct dphy_mbps_hsfreqrange_map *prev_value = NULL;
+	const struct dphy_mbps_hsfreqrange_map *value;
+
+	for (value = hsfreqrange_table; value->mbps; value++) {
+		if (value->mbps >= mbps)
+			break;
+		prev_value = value;
+	}
+
+	if (prev_value &&
+	    ((mbps - prev_value->mbps) <= (value->mbps - mbps)))
+		value = prev_value;
+
+	if (!value->mbps) {
+		pr_err("Unsupported PHY speed (%llu Mbps)", mbps);
+		return -ERANGE;
+	}
+
+	priv->hsfreqrange = value->hsfreqrange;
+	priv->ddlfreq = value->ddlfreq;
+
+	return 0;
+}
+
+static int fsl_csi2_phy_configure(struct phy *phy, union phy_configure_opts *opts)
+{
+	struct fsl_csi2_phy *priv = phy_get_drvdata(phy);
+	const struct fsl_csi2_phy_drv_data *drv_data = priv->drv_data;
+	struct phy_configure_opts_mipi_dphy *config = &opts->mipi_dphy;
+	struct device *dev = priv->dev;
+	u64 data_rate_mbps;
+	int ret;
+
+	if (config->lanes > drv_data->max_lanes) {
+		dev_err(dev, "The number of lanes has exceeded the maximum value\n");
+		return -EINVAL;
+	}
+
+	data_rate_mbps = div_u64(config->hs_clk_rate, 1000 * 1000);
+	if (data_rate_mbps < 80 ||
+	    data_rate_mbps > drv_data->max_data_rate) {
+		dev_err(dev, "Out-of-bound lane rate %llu\n", data_rate_mbps);
+		return -EINVAL;
+	}
+
+	dev_dbg(dev, "Number of lanes: %d, data rate=%llu(Mbps)\n",
+		config->lanes, data_rate_mbps);
+
+	ret = set_freqrange_by_mpbs(priv, data_rate_mbps);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static const struct phy_ops fsl_csi2_phy_ops = {
+	.init = fsl_csi2_phy_init,
+	.exit = fsl_csi2_phy_exit,
+	.power_on = fsl_csi2_phy_power_on,
+	.configure = fsl_csi2_phy_configure,
+	.owner = THIS_MODULE,
+};
+
+static const struct fsl_csi2_phy_drv_data imx93_dphy_drvdata = {
+	.max_lanes = 2,
+	.max_data_rate = 1500,
+};
+
+static int fsl_csi2_runtime_suspend(struct device *dev)
+{
+	struct fsl_csi2_phy *priv = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(priv->cfg_clk);
+
+	return 0;
+}
+
+static int fsl_csi2_runtime_resume(struct device *dev)
+{
+	struct fsl_csi2_phy *priv = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_prepare_enable(priv->cfg_clk);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static DEFINE_RUNTIME_DEV_PM_OPS(fsl_csi2_pm_ops, fsl_csi2_runtime_suspend,
+				 fsl_csi2_runtime_resume, NULL);
+
+static const struct of_device_id fsl_csi2_phy_of_match[] = {
+	{ .compatible = "fsl,imx93-dphy-rx", .data = &imx93_dphy_drvdata},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, fsl_csi2_phy_of_match);
+
+static int fsl_csi2_phy_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct phy_provider *phy_provider;
+	struct fsl_csi2_phy *priv;
+	unsigned long cfg_rate;
+	struct phy *phy;
+
+	if (!dev->parent || !dev->parent->of_node)
+		return -ENODEV;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->dev = dev;
+	priv->drv_data = of_device_get_match_data(dev);
+
+	platform_set_drvdata(pdev, priv);
+
+	priv->dphy_regmap = syscon_node_to_regmap(dev->parent->of_node);
+	if (IS_ERR(priv->dphy_regmap))
+		dev_err_probe(dev, -ENODEV, "Failed to DPHY regmap\n");
+
+	priv->cfg_clk = devm_clk_get(dev, "cfg");
+	if (IS_ERR(priv->cfg_clk))
+		dev_err_probe(dev, PTR_ERR(priv->cfg_clk), "Failed to get DPHY config clock\n");
+
+	/* cfgclkfreqrange[5:0] = round[(cfg_clk(MHz) - 17) * 4] */
+	cfg_rate = clk_get_rate(priv->cfg_clk);
+	if (!cfg_rate)
+		dev_err_probe(dev, -EINVAL, "Failed to get PHY config clock rate\n");
+
+	priv->cfgclkfreqrange = (div_u64(cfg_rate, 1000 * 1000) - 17) * 4;
+
+	phy = devm_phy_create(dev, np, &fsl_csi2_phy_ops);
+	if (IS_ERR(phy))
+		return dev_err_probe(dev, -ENODEV, "Failed to create PHY\n");
+
+	phy_set_drvdata(phy, priv);
+
+	pm_runtime_set_suspended(dev);
+	devm_pm_runtime_enable(dev);
+
+	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+
+	return PTR_ERR_OR_ZERO(phy_provider);
+}
+
+static struct platform_driver fsl_csi2_phy_driver = {
+	.probe	= fsl_csi2_phy_probe,
+	.driver = {
+		.name = "imx-mipi-dphy-rx",
+		.pm = pm_ptr(&fsl_csi2_pm_ops),
+		.of_match_table	= fsl_csi2_phy_of_match,
+	}
+};
+module_platform_driver(fsl_csi2_phy_driver);
+
+MODULE_DESCRIPTION("i.MX9 Synopsys DesignWare MIPI DPHY Rx wrapper driver");
+MODULE_AUTHOR("NXP Semiconductor");
+MODULE_LICENSE("GPL");