From 7e328f5adcabb0b08e713f202b4a0fc388b2319e Mon Sep 17 00:00:00 2001 From: Tomi Valkeinen Date: Thu, 4 Jun 2015 13:02:52 +0300 Subject: [PATCH] OMAPDSS: refactor dss probe function Refactor dss probe function by extracting the setup for video plls into a separate function. The call to this function is also moved to a slightly earlier phase, so that in error case we can bail out more easily. Signed-off-by: Tomi Valkeinen Acked-by: Laurent Pinchart --- drivers/video/fbdev/omap2/dss/dss.c | 147 +++++++++++++++------------- 1 file changed, 80 insertions(+), 67 deletions(-) diff --git a/drivers/video/fbdev/omap2/dss/dss.c b/drivers/video/fbdev/omap2/dss/dss.c index 35601ab232e3..1ce47441efe3 100644 --- a/drivers/video/fbdev/omap2/dss/dss.c +++ b/drivers/video/fbdev/omap2/dss/dss.c @@ -1026,69 +1026,16 @@ static void __exit dss_uninit_ports(struct platform_device *pdev) } while ((port = omapdss_of_get_next_port(parent, port)) != NULL); } -/* DSS HW IP initialisation */ -static int __init omap_dsshw_probe(struct platform_device *pdev) +static int dss_video_pll_probe(struct platform_device *pdev) { - struct resource *dss_mem; struct device_node *np = pdev->dev.of_node; - u32 rev; - int r; struct regulator *pll_regulator; + int r; - dss.pdev = pdev; + if (!np) + return 0; - r = dss_init_features(dss.pdev); - if (r) - return r; - - dss_mem = platform_get_resource(dss.pdev, IORESOURCE_MEM, 0); - if (!dss_mem) { - DSSERR("can't get IORESOURCE_MEM DSS\n"); - return -EINVAL; - } - - dss.base = devm_ioremap(&pdev->dev, dss_mem->start, - resource_size(dss_mem)); - if (!dss.base) { - DSSERR("can't ioremap DSS\n"); - return -ENOMEM; - } - - r = dss_get_clocks(); - if (r) - return r; - - r = dss_setup_default_clock(); - if (r) - goto err_setup_clocks; - - pm_runtime_enable(&pdev->dev); - - r = dss_runtime_get(); - if (r) - goto err_runtime_get; - - dss.dss_clk_rate = clk_get_rate(dss.dss_clk); - - /* Select DPLL */ - REG_FLD_MOD(DSS_CONTROL, 0, 0, 0); - - dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK); - -#ifdef CONFIG_OMAP2_DSS_VENC - REG_FLD_MOD(DSS_CONTROL, 1, 4, 4); /* venc dac demen */ - REG_FLD_MOD(DSS_CONTROL, 1, 3, 3); /* venc clock 4x enable */ - REG_FLD_MOD(DSS_CONTROL, 0, 2, 2); /* venc clock mode = normal */ -#endif - dss.dsi_clk_source[0] = OMAP_DSS_CLK_SRC_FCK; - dss.dsi_clk_source[1] = OMAP_DSS_CLK_SRC_FCK; - dss.dispc_clk_source = OMAP_DSS_CLK_SRC_FCK; - dss.lcd_clk_source[0] = OMAP_DSS_CLK_SRC_FCK; - dss.lcd_clk_source[1] = OMAP_DSS_CLK_SRC_FCK; - - dss_init_ports(pdev); - - if (np && of_property_read_bool(np, "syscon-pll-ctrl")) { + if (of_property_read_bool(np, "syscon-pll-ctrl")) { dss.syscon_pll_ctrl = syscon_regmap_lookup_by_phandle(np, "syscon-pll-ctrl"); if (IS_ERR(dss.syscon_pll_ctrl)) { @@ -1125,20 +1072,85 @@ static int __init omap_dsshw_probe(struct platform_device *pdev) if (of_property_match_string(np, "reg-names", "pll1") >= 0) { dss.video1_pll = dss_video_pll_init(pdev, 0, pll_regulator); - if (IS_ERR(dss.video1_pll)) { - r = PTR_ERR(dss.video1_pll); - goto err_pll_init; - } + if (IS_ERR(dss.video1_pll)) + return PTR_ERR(dss.video1_pll); } if (of_property_match_string(np, "reg-names", "pll2") >= 0) { dss.video2_pll = dss_video_pll_init(pdev, 1, pll_regulator); if (IS_ERR(dss.video2_pll)) { - r = PTR_ERR(dss.video2_pll); - goto err_pll_init; + dss_video_pll_uninit(dss.video1_pll); + return PTR_ERR(dss.video2_pll); } } + return 0; +} + +/* DSS HW IP initialisation */ +static int __init omap_dsshw_probe(struct platform_device *pdev) +{ + struct resource *dss_mem; + u32 rev; + int r; + + dss.pdev = pdev; + + r = dss_init_features(dss.pdev); + if (r) + return r; + + dss_mem = platform_get_resource(dss.pdev, IORESOURCE_MEM, 0); + if (!dss_mem) { + DSSERR("can't get IORESOURCE_MEM DSS\n"); + return -EINVAL; + } + + dss.base = devm_ioremap(&pdev->dev, dss_mem->start, + resource_size(dss_mem)); + if (!dss.base) { + DSSERR("can't ioremap DSS\n"); + return -ENOMEM; + } + + r = dss_get_clocks(); + if (r) + return r; + + r = dss_setup_default_clock(); + if (r) + goto err_setup_clocks; + + r = dss_video_pll_probe(pdev); + if (r) + goto err_pll_init; + + pm_runtime_enable(&pdev->dev); + + r = dss_runtime_get(); + if (r) + goto err_runtime_get; + + dss.dss_clk_rate = clk_get_rate(dss.dss_clk); + + /* Select DPLL */ + REG_FLD_MOD(DSS_CONTROL, 0, 0, 0); + + dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK); + +#ifdef CONFIG_OMAP2_DSS_VENC + REG_FLD_MOD(DSS_CONTROL, 1, 4, 4); /* venc dac demen */ + REG_FLD_MOD(DSS_CONTROL, 1, 3, 3); /* venc clock 4x enable */ + REG_FLD_MOD(DSS_CONTROL, 0, 2, 2); /* venc clock mode = normal */ +#endif + dss.dsi_clk_source[0] = OMAP_DSS_CLK_SRC_FCK; + dss.dsi_clk_source[1] = OMAP_DSS_CLK_SRC_FCK; + dss.dispc_clk_source = OMAP_DSS_CLK_SRC_FCK; + dss.lcd_clk_source[0] = OMAP_DSS_CLK_SRC_FCK; + dss.lcd_clk_source[1] = OMAP_DSS_CLK_SRC_FCK; + + dss_init_ports(pdev); + rev = dss_read_reg(DSS_REVISION); printk(KERN_INFO "OMAP DSS rev %d.%d\n", FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0)); @@ -1153,14 +1165,15 @@ static int __init omap_dsshw_probe(struct platform_device *pdev) return 0; -err_pll_init: +err_runtime_get: + pm_runtime_disable(&pdev->dev); + if (dss.video1_pll) dss_video_pll_uninit(dss.video1_pll); if (dss.video2_pll) dss_video_pll_uninit(dss.video2_pll); -err_runtime_get: - pm_runtime_disable(&pdev->dev); +err_pll_init: err_setup_clocks: dss_put_clocks(); return r;