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) |