diff --git a/drivers/gpu/drm/armada/armada_510.c b/drivers/gpu/drm/armada/armada_510.c
index 370c422f64e3..6f8ad8fb19f1 100644
--- a/drivers/gpu/drm/armada/armada_510.c
+++ b/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;
 
-	clk = devm_clk_get(dev, "ext_ref_clk1");
-	if (IS_ERR(clk))
-		return PTR_ERR(clk) == -ENOENT ? -EPROBE_DEFER : PTR_ERR(clk);
+	v = devm_kzalloc(dev, sizeof(*v), GFP_KERNEL);
+	if (!v)
+		return -ENOMEM;
 
-	dcrtc->extclk[0] = clk;
+	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);
+
+		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;
+	struct armada510_variant_data *v = dcrtc->variant_data;
+	unsigned long desired_khz = mode->crtc_clock;
+	struct armada_clk_result res;
+	int ret, idx;
 
-	if (dcrtc->num == 1)
-		return -EINVAL;
+	idx = armada_crtc_select_clock(dcrtc, &res, &armada510_clocking,
+				       v->clks, ARRAY_SIZE(v->clks),
+				       desired_khz);
+	if (idx < 0)
+		return idx;
 
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
-
-	if (dcrtc->clk != clk) {
-		ret = clk_prepare_enable(clk);
-		if (ret)
-			return ret;
-		dcrtc->clk = clk;
-	}
+	ret = clk_prepare_enable(res.clk);
+	if (ret)
+		return ret;
 
 	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;
 	}
 }
 
diff --git a/drivers/gpu/drm/armada/armada_crtc.c b/drivers/gpu/drm/armada/armada_crtc.c
index edce74f60198..cffb4c31e9d3 100644
--- a/drivers/gpu/drm/armada/armada_crtc.c
+++ b/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);
diff --git a/drivers/gpu/drm/armada/armada_crtc.h b/drivers/gpu/drm/armada/armada_crtc.h
index 08761ff01739..fb4aa48da60c 100644
--- a/drivers/gpu/drm/armada/armada_crtc.h
+++ b/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