drm/armada: improve Dove clock selection

Improve the Dove (Armada 510) LCD clock selection and divider
calculation, limiting to the valid divisor values, and reporting an
error if the clock is not achievable within the bounds of HDMI
clocking requirements.
Signed-off-by: Russell King <rmk+kernel@armlinux.org.uk>

drivers/gpu/drm/armada/armada_510.c

@@ -14,15 +14,54 @@
 #include "armada_drm.h"
 #include "armada_hw.h"
 
+struct armada510_variant_data {
+	struct clk *clks[4];
+	struct clk *sel_clk;
+};
+
 static int armada510_crtc_init(struct armada_crtc *dcrtc, struct device *dev)
 {
+	struct armada510_variant_data *v;
 	struct clk *clk;
-
+	int idx;
+
+	v = devm_kzalloc(dev, sizeof(*v), GFP_KERNEL);
+	if (!v)
+		return -ENOMEM;
+
+	dcrtc->variant_data = v;
+
+	if (dev->of_node) {
+		struct property *prop;
+		const char *s;
+
+		of_property_for_each_string(dev->of_node, "clock-names", prop,
+					    s) {
+			if (!strcmp(s, "ext_ref_clk0"))
+				idx = 0;
+			else if (!strcmp(s, "ext_ref_clk1"))
+				idx = 1;
+			else if (!strcmp(s, "plldivider"))
+				idx = 2;
+			else if (!strcmp(s, "axibus"))
+				idx = 3;
+			else
+				continue;
+
+			clk = devm_clk_get(dev, s);
+			if (IS_ERR(clk))
+				return PTR_ERR(clk) == -ENOENT ? -EPROBE_DEFER :
+					PTR_ERR(clk);
+			v->clks[idx] = clk;
+		}
+	} else {
 		clk = devm_clk_get(dev, "ext_ref_clk1");
 		if (IS_ERR(clk))
-		return PTR_ERR(clk) == -ENOENT ? -EPROBE_DEFER : PTR_ERR(clk);
+			return PTR_ERR(clk) == -ENOENT ? -EPROBE_DEFER :
+				PTR_ERR(clk);
 
-	dcrtc->extclk[0] = clk;
+		v->clks[1] = clk;
+	}
 
 	/*
 	 * Lower the watermark so to eliminate jitter at higher bandwidths.
@@ -39,65 +78,77 @@ static int armada510_crtc_init(struct armada_crtc *dcrtc, struct device *dev)
 	return 0;
 }
 
+static const u32 armada510_clk_sels[] = {
+	SCLK_510_EXTCLK0,
+	SCLK_510_EXTCLK1,
+	SCLK_510_PLL,
+	SCLK_510_AXI,
+};
+
+static const struct armada_clocking_params armada510_clocking = {
+	/* HDMI requires -0.6%..+0.5% */
+	.permillage_min = 994,
+	.permillage_max = 1005,
+	.settable = BIT(0) | BIT(1),
+	.div_max = SCLK_510_INT_DIV_MASK,
+};
+
 /*
  * Armada510 specific SCLK register selection.
  * This gets called with sclk = NULL to test whether the mode is
  * supportable, and again with sclk != NULL to set the clocks up for
  * that.  The former can return an error, but the latter is expected
  * not to.
- *
- * We currently are pretty rudimentary here, always selecting
- * EXT_REF_CLK_1 for LCD0 and erroring LCD1.  This needs improvement!
  */
 static int armada510_crtc_compute_clock(struct armada_crtc *dcrtc,
 	const struct drm_display_mode *mode, uint32_t *sclk)
 {
-	struct clk *clk = dcrtc->extclk[0];
-	int ret;
-
-	if (dcrtc->num == 1)
-		return -EINVAL;
-
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
-
-	if (dcrtc->clk != clk) {
-		ret = clk_prepare_enable(clk);
+	struct armada510_variant_data *v = dcrtc->variant_data;
+	unsigned long desired_khz = mode->crtc_clock;
+	struct armada_clk_result res;
+	int ret, idx;
+
+	idx = armada_crtc_select_clock(dcrtc, &res, &armada510_clocking,
+				       v->clks, ARRAY_SIZE(v->clks),
+				       desired_khz);
+	if (idx < 0)
+		return idx;
+
+	ret = clk_prepare_enable(res.clk);
 	if (ret)
 		return ret;
-		dcrtc->clk = clk;
-	}
 
 	if (sclk) {
-		uint32_t rate, ref, div;
+		clk_set_rate(res.clk, res.desired_clk_hz);
 
-		rate = mode->clock * 1000;
-		ref = clk_round_rate(clk, rate);
-		div = DIV_ROUND_UP(ref, rate);
-		if (div < 1)
-			div = 1;
+		*sclk = res.div | armada510_clk_sels[idx];
 
-		clk_set_rate(clk, ref);
-		*sclk = div | SCLK_510_EXTCLK1;
+		/* We are now using this clock */
+		v->sel_clk = res.clk;
+		swap(dcrtc->clk, res.clk);
 	}
 
+	clk_disable_unprepare(res.clk);
+
 	return 0;
 }
 
 static void armada510_crtc_disable(struct armada_crtc *dcrtc)
 {
-	if (!IS_ERR(dcrtc->clk)) {
+	if (dcrtc->clk) {
 		clk_disable_unprepare(dcrtc->clk);
-		dcrtc->clk = ERR_PTR(-EINVAL);
+		dcrtc->clk = NULL;
 	}
 }
 
 static void armada510_crtc_enable(struct armada_crtc *dcrtc,
 	const struct drm_display_mode *mode)
 {
-	if (IS_ERR(dcrtc->clk)) {
-		dcrtc->clk = dcrtc->extclk[0];
-		WARN_ON(clk_prepare_enable(dcrtc->clk));
+	struct armada510_variant_data *v = dcrtc->variant_data;
+
+	if (!dcrtc->clk && v->sel_clk) {
+		if (!WARN_ON(clk_prepare_enable(v->sel_clk)))
+			dcrtc->clk = v->sel_clk;
 	}
 }
 

drivers/gpu/drm/armada/armada_crtc.c

@@ -801,6 +801,83 @@ static const struct drm_crtc_funcs armada_crtc_funcs = {
 	.disable_vblank	= armada_drm_crtc_disable_vblank,
 };
 
+int armada_crtc_select_clock(struct armada_crtc *dcrtc,
+			     struct armada_clk_result *res,
+			     const struct armada_clocking_params *params,
+			     struct clk *clks[], size_t num_clks,
+			     unsigned long desired_khz)
+{
+	unsigned long desired_hz = desired_khz * 1000;
+	unsigned long desired_clk_hz;	// requested clk input
+	unsigned long real_clk_hz;	// actual clk input
+	unsigned long real_hz;		// actual pixel clk
+	unsigned long permillage;
+	struct clk *clk;
+	u32 div;
+	int i;
+
+	DRM_DEBUG_KMS("[CRTC:%u:%s] desired clock=%luHz\n",
+		      dcrtc->crtc.base.id, dcrtc->crtc.name, desired_hz);
+
+	for (i = 0; i < num_clks; i++) {
+		clk = clks[i];
+		if (!clk)
+			continue;
+
+		if (params->settable & BIT(i)) {
+			real_clk_hz = clk_round_rate(clk, desired_hz);
+			desired_clk_hz = desired_hz;
+		} else {
+			real_clk_hz = clk_get_rate(clk);
+			desired_clk_hz = real_clk_hz;
+		}
+
+		/* If the clock can do exactly the desired rate, we're done */
+		if (real_clk_hz == desired_hz) {
+			real_hz = real_clk_hz;
+			div = 1;
+			goto found;
+		}
+
+		/* Calculate the divider - if invalid, we can't do this rate */
+		div = DIV_ROUND_CLOSEST(real_clk_hz, desired_hz);
+		if (div == 0 || div > params->div_max)
+			continue;
+
+		/* Calculate the actual rate - HDMI requires -0.6%..+0.5% */
+		real_hz = DIV_ROUND_CLOSEST(real_clk_hz, div);
+
+		DRM_DEBUG_KMS("[CRTC:%u:%s] clk=%u %luHz div=%u real=%luHz\n",
+			dcrtc->crtc.base.id, dcrtc->crtc.name,
+			i, real_clk_hz, div, real_hz);
+
+		/* Avoid repeated division */
+		if (real_hz < desired_hz) {
+			permillage = real_hz / desired_khz;
+			if (permillage < params->permillage_min)
+				continue;
+		} else {
+			permillage = DIV_ROUND_UP(real_hz, desired_khz);
+			if (permillage > params->permillage_max)
+				continue;
+		}
+		goto found;
+	}
+
+	return -ERANGE;
+
+found:
+	DRM_DEBUG_KMS("[CRTC:%u:%s] selected clk=%u %luHz div=%u real=%luHz\n",
+		dcrtc->crtc.base.id, dcrtc->crtc.name,
+		i, real_clk_hz, div, real_hz);
+
+	res->desired_clk_hz = desired_clk_hz;
+	res->clk = clk;
+	res->div = div;
+
+	return i;
+}
+
 static int armada_drm_crtc_create(struct drm_device *drm, struct device *dev,
 	struct resource *res, int irq, const struct armada_variant *variant,
 	struct device_node *port)
@@ -827,7 +904,6 @@ static int armada_drm_crtc_create(struct drm_device *drm, struct device *dev,
 	dcrtc->variant = variant;
 	dcrtc->base = base;
 	dcrtc->num = drm->mode_config.num_crtc;
-	dcrtc->clk = ERR_PTR(-EINVAL);
 	dcrtc->cfg_dumb_ctrl = DUMB24_RGB888_0;
 	dcrtc->spu_iopad_ctrl = CFG_VSCALE_LN_EN | CFG_IOPAD_DUMB24;
 	spin_lock_init(&dcrtc->irq_lock);

drivers/gpu/drm/armada/armada_crtc.h

@@ -39,10 +39,10 @@ struct armada_variant;
 struct armada_crtc {
 	struct drm_crtc		crtc;
 	const struct armada_variant *variant;
+	void			*variant_data;
 	unsigned		num;
 	void __iomem		*base;
 	struct clk		*clk;
-	struct clk		*extclk[2];
 	struct {
 		uint32_t	spu_v_h_total;
 		uint32_t	spu_v_porch;
@@ -75,6 +75,25 @@ struct armada_crtc {
 
 void armada_drm_crtc_update_regs(struct armada_crtc *, struct armada_regs *);
 
+struct armada_clocking_params {
+	unsigned long permillage_min;
+	unsigned long permillage_max;
+	u32 settable;
+	u32 div_max;
+};
+
+struct armada_clk_result {
+	unsigned long desired_clk_hz;
+	struct clk *clk;
+	u32 div;
+};
+
+int armada_crtc_select_clock(struct armada_crtc *dcrtc,
+			     struct armada_clk_result *res,
+			     const struct armada_clocking_params *params,
+			     struct clk *clks[], size_t num_clks,
+			     unsigned long desired_khz);
+
 extern struct platform_driver armada_lcd_platform_driver;
 
 #endif