1 files changed, 65 insertions, 20 deletions

diff --git a/drivers/video/da8xx-fb.c b/drivers/video/da8xx-fb.c
index d8b295ab85f5..dfe757254e26 100644
--- a/drivers/video/da8xx-fb.c
+++ b/drivers/video/da8xx-fb.c
@@ -132,6 +132,9 @@
 #define WSI_TIMEOUT	50
 #define PALETTE_SIZE	256
 
+#define	CLK_MIN_DIV	2
+#define	CLK_MAX_DIV	255
+
 static void __iomem *da8xx_fb_reg_base;
 static unsigned int lcd_revision;
 static irq_handler_t lcdc_irq_handler;
@@ -684,38 +687,76 @@ static void da8xx_fb_lcd_reset(void)
 	}
 }
 
-static inline unsigned da8xx_fb_calc_clk_divider(struct da8xx_fb_par *par,
-						 unsigned pixclock)
-{
-	return par->lcd_fck_rate / (PICOS2KHZ(pixclock) * 1000);
-}
-
-static inline unsigned da8xx_fb_round_clk(struct da8xx_fb_par *par,
-					  unsigned pixclock)
+static int da8xx_fb_config_clk_divider(struct da8xx_fb_par *par,
+					      unsigned lcdc_clk_div,
+					      unsigned lcdc_clk_rate)
 {
-	unsigned div;
+	int ret;
 
-	div = da8xx_fb_calc_clk_divider(par, pixclock);
-	return KHZ2PICOS(par->lcd_fck_rate / (1000 * div));
-}
+	if (par->lcd_fck_rate != lcdc_clk_rate) {
+		ret = clk_set_rate(par->lcdc_clk, lcdc_clk_rate);
+		if (IS_ERR_VALUE(ret)) {
+			dev_err(par->dev,
+				"unable to set clock rate at %u\n",
+				lcdc_clk_rate);
+			return ret;
+		}
+		par->lcd_fck_rate = clk_get_rate(par->lcdc_clk);
+	}
 
-static inline void da8xx_fb_config_clk_divider(unsigned div)
-{
 	/* Configure the LCD clock divisor. */
-	lcdc_write(LCD_CLK_DIVISOR(div) |
+	lcdc_write(LCD_CLK_DIVISOR(lcdc_clk_div) |
 			(LCD_RASTER_MODE & 0x1), LCD_CTRL_REG);
 
 	if (lcd_revision == LCD_VERSION_2)
 		lcdc_write(LCD_V2_DMA_CLK_EN | LCD_V2_LIDD_CLK_EN |
 				LCD_V2_CORE_CLK_EN, LCD_CLK_ENABLE_REG);
+
+	return 0;
+}
+
+static unsigned int da8xx_fb_calc_clk_divider(struct da8xx_fb_par *par,
+					      unsigned pixclock,
+					      unsigned *lcdc_clk_rate)
+{
+	unsigned lcdc_clk_div;
+
+	pixclock = PICOS2KHZ(pixclock) * 1000;
+
+	*lcdc_clk_rate = par->lcd_fck_rate;
+
+	if (pixclock < (*lcdc_clk_rate / CLK_MAX_DIV)) {
+		*lcdc_clk_rate = clk_round_rate(par->lcdc_clk,
+						pixclock * CLK_MAX_DIV);
+		lcdc_clk_div = CLK_MAX_DIV;
+	} else if (pixclock > (*lcdc_clk_rate / CLK_MIN_DIV)) {
+		*lcdc_clk_rate = clk_round_rate(par->lcdc_clk,
+						pixclock * CLK_MIN_DIV);
+		lcdc_clk_div = CLK_MIN_DIV;
+	} else {
+		lcdc_clk_div = *lcdc_clk_rate / pixclock;
+	}
+
+	return lcdc_clk_div;
 }
 
-static inline void da8xx_fb_calc_config_clk_divider(struct da8xx_fb_par *par,
-						    struct fb_videomode *mode)
+static int da8xx_fb_calc_config_clk_divider(struct da8xx_fb_par *par,
+					    struct fb_videomode *mode)
 {
-	unsigned div = da8xx_fb_calc_clk_divider(par, mode->pixclock);
+	unsigned lcdc_clk_rate;
+	unsigned lcdc_clk_div = da8xx_fb_calc_clk_divider(par, mode->pixclock,
+							  &lcdc_clk_rate);
 
-	da8xx_fb_config_clk_divider(div);
+	return da8xx_fb_config_clk_divider(par, lcdc_clk_div, lcdc_clk_rate);
+}
+
+static inline unsigned da8xx_fb_round_clk(struct da8xx_fb_par *par,
+					  unsigned pixclock)
+{
+	unsigned lcdc_clk_div, lcdc_clk_rate;
+
+	lcdc_clk_div = da8xx_fb_calc_clk_divider(par, pixclock, &lcdc_clk_rate);
+	return KHZ2PICOS(lcdc_clk_rate / (1000 * lcdc_clk_div));
 }
 
 static int lcd_init(struct da8xx_fb_par *par, const struct lcd_ctrl_config *cfg,
@@ -724,7 +765,11 @@ static int lcd_init(struct da8xx_fb_par *par, const struct lcd_ctrl_config *cfg,
 	u32 bpp;
 	int ret = 0;
 
-	da8xx_fb_calc_config_clk_divider(par, panel);
+	ret = da8xx_fb_calc_config_clk_divider(par, panel);
+	if (IS_ERR_VALUE(ret)) {
+		dev_err(par->dev, "unable to configure clock\n");
+		return ret;
+	}
 
 	if (panel->sync & FB_SYNC_CLK_INVERT)
 		lcdc_write((lcdc_read(LCD_RASTER_TIMING_2_REG) |