1 files changed, 54 insertions, 14 deletions

diff --git a/drivers/video/fbdev/mxsfb.c b/drivers/video/fbdev/mxsfb.c
index f8ac4a452f26..0f64165b0147 100644
--- a/drivers/video/fbdev/mxsfb.c
+++ b/drivers/video/fbdev/mxsfb.c
@@ -316,6 +316,18 @@ static int mxsfb_check_var(struct fb_var_screeninfo *var,
         return 0;
 }
 
+static inline void mxsfb_enable_axi_clk(struct mxsfb_info *host)
+{
+        if (host->clk_axi)
+                clk_prepare_enable(host->clk_axi);
+}
+
+static inline void mxsfb_disable_axi_clk(struct mxsfb_info *host)
+{
+        if (host->clk_axi)
+                clk_disable_unprepare(host->clk_axi);
+}
+
 static void mxsfb_enable_controller(struct fb_info *fb_info)
 {
         struct mxsfb_info *host = to_imxfb_host(fb_info);
@@ -333,14 +345,13 @@ static void mxsfb_enable_controller(struct fb_info *fb_info)
                 }
         }
 
-        if (host->clk_axi)
-                clk_prepare_enable(host->clk_axi);
-
         if (host->clk_disp_axi)
                 clk_prepare_enable(host->clk_disp_axi);
         clk_prepare_enable(host->clk);
         clk_set_rate(host->clk, PICOS2KHZ(fb_info->var.pixclock) * 1000U);
 
+        mxsfb_enable_axi_clk(host);
+
         /* if it was disabled, re-enable the mode again */
         writel(CTRL_DOTCLK_MODE, host->base + LCDC_CTRL + REG_SET);
 
@@ -380,11 +391,11 @@ static void mxsfb_disable_controller(struct fb_info *fb_info)
         reg = readl(host->base + LCDC_VDCTRL4);
         writel(reg & ~VDCTRL4_SYNC_SIGNALS_ON, host->base + LCDC_VDCTRL4);
 
+        mxsfb_disable_axi_clk(host);
+
         clk_disable_unprepare(host->clk);
         if (host->clk_disp_axi)
                 clk_disable_unprepare(host->clk_disp_axi);
-        if (host->clk_axi)
-                clk_disable_unprepare(host->clk_axi);
 
         host->enabled = 0;
 
@@ -421,6 +432,8 @@ static int mxsfb_set_par(struct fb_info *fb_info)
                 mxsfb_disable_controller(fb_info);
         }
 
+        mxsfb_enable_axi_clk(host);
+
         /* clear the FIFOs */
         writel(CTRL1_FIFO_CLEAR, host->base + LCDC_CTRL1 + REG_SET);
 
@@ -438,6 +451,7 @@ static int mxsfb_set_par(struct fb_info *fb_info)
                 ctrl |= CTRL_SET_WORD_LENGTH(3);
                 switch (host->ld_intf_width) {
                 case STMLCDIF_8BIT:
+                        mxsfb_disable_axi_clk(host);
                         dev_err(&host->pdev->dev,
                                         "Unsupported LCD bus width mapping\n");
                         return -EINVAL;
@@ -451,6 +465,7 @@ static int mxsfb_set_par(struct fb_info *fb_info)
                 writel(CTRL1_SET_BYTE_PACKAGING(0x7), host->base + LCDC_CTRL1);
                 break;
         default:
+                mxsfb_disable_axi_clk(host);
                 dev_err(&host->pdev->dev, "Unhandled color depth of %u\n",
                                 fb_info->var.bits_per_pixel);
                 return -EINVAL;
@@ -504,6 +519,8 @@ static int mxsfb_set_par(struct fb_info *fb_info)
                         fb_info->fix.line_length * fb_info->var.yoffset,
                         host->base + host->devdata->next_buf);
 
+        mxsfb_disable_axi_clk(host);
+
         if (reenable)
                 mxsfb_enable_controller(fb_info);
 
@@ -582,10 +599,14 @@ static int mxsfb_pan_display(struct fb_var_screeninfo *var,
 
         offset = fb_info->fix.line_length * var->yoffset;
 
+        mxsfb_enable_axi_clk(host);
+
         /* update on next VSYNC */
         writel(fb_info->fix.smem_start + offset,
                         host->base + host->devdata->next_buf);
 
+        mxsfb_disable_axi_clk(host);
+
         return 0;
 }
 
@@ -608,13 +629,17 @@ static int mxsfb_restore_mode(struct mxsfb_info *host,
         unsigned line_count;
         unsigned period;
         unsigned long pa, fbsize;
-        int bits_per_pixel, ofs;
+        int bits_per_pixel, ofs, ret = 0;
         u32 transfer_count, vdctrl0, vdctrl2, vdctrl3, vdctrl4, ctrl;
 
+        mxsfb_enable_axi_clk(host);
+
         /* Only restore the mode when the controller is running */
         ctrl = readl(host->base + LCDC_CTRL);
-        if (!(ctrl & CTRL_RUN))
-                return -EINVAL;
+        if (!(ctrl & CTRL_RUN)) {
+                ret = -EINVAL;
+                goto err;
+        }
 
         vdctrl0 = readl(host->base + LCDC_VDCTRL0);
         vdctrl2 = readl(host->base + LCDC_VDCTRL2);
@@ -635,7 +660,8 @@ static int mxsfb_restore_mode(struct mxsfb_info *host,
                 break;
         case 1:
         default:
-                return -EINVAL;
+                ret = -EINVAL;
+                goto err;
         }
 
         fb_info->var.bits_per_pixel = bits_per_pixel;
@@ -673,10 +699,14 @@ static int mxsfb_restore_mode(struct mxsfb_info *host,
 
         pa = readl(host->base + host->devdata->cur_buf);
         fbsize = fb_info->fix.line_length * vmode->yres;
-        if (pa < fb_info->fix.smem_start)
-                return -EINVAL;
-        if (pa + fbsize > fb_info->fix.smem_start + fb_info->fix.smem_len)
-                return -EINVAL;
+        if (pa < fb_info->fix.smem_start) {
+                ret = -EINVAL;
+                goto err;
+        }
+        if (pa + fbsize > fb_info->fix.smem_start + fb_info->fix.smem_len) {
+                ret = -EINVAL;
+                goto err;
+        }
         ofs = pa - fb_info->fix.smem_start;
         if (ofs) {
                 memmove(fb_info->screen_base, fb_info->screen_base + ofs, fbsize);
@@ -689,7 +719,11 @@ static int mxsfb_restore_mode(struct mxsfb_info *host,
         clk_prepare_enable(host->clk);
         host->enabled = 1;
 
-        return 0;
+err:
+        if (ret)
+                mxsfb_disable_axi_clk(host);
+
+        return ret;
 }
 
 static int mxsfb_init_fbinfo_dt(struct mxsfb_info *host,
@@ -915,7 +949,9 @@ static int mxsfb_probe(struct platform_device *pdev)
         }
 
         if (!host->enabled) {
+                mxsfb_enable_axi_clk(host);
                 writel(0, host->base + LCDC_CTRL);
+                mxsfb_disable_axi_clk(host);
                 mxsfb_set_par(fb_info);
                 mxsfb_enable_controller(fb_info);
         }
@@ -954,11 +990,15 @@ static void mxsfb_shutdown(struct platform_device *pdev)
         struct fb_info *fb_info = platform_get_drvdata(pdev);
         struct mxsfb_info *host = to_imxfb_host(fb_info);
 
+        mxsfb_enable_axi_clk(host);
+
         /*
          * Force stop the LCD controller as keeping it running during reboot
          * might interfere with the BootROM's boot mode pads sampling.
          */
         writel(CTRL_RUN, host->base + LCDC_CTRL + REG_CLR);
+
+        mxsfb_disable_axi_clk(host);
 }
 
 static struct platform_driver mxsfb_driver = {