video: mxsfb: refactor video_hw_init()

Refactor video_hw_init() function, and introduce an independent function
for the common procedure of initialization.

Currently video_hw_init() is only in charge of parsing configuration from
env("videomode") and filling struct GraphicPanel, and new
mxs_probe_common() does hw specific initialization (invocation of
mxs_lcd_init() etc.)

Signed-off-by: Igor Opaniuk <igor.opaniuk@toradex.com>

diff --git a/drivers/video/mxsfb.c b/drivers/video/mxsfb.c
index 059fbf2b05dc6bf08133532190b86df6fe9a311a..6e269409d6691c21bd36c0a3561323dbb1337d58 100644 (file)
--- a/drivers/video/mxsfb.c
+++ b/drivers/video/mxsfb.c
@@ -128,6 +128,37 @@ static void mxs_lcd_init(u32 fb_addr, struct ctfb_res_modes *mode, int bpp)
        writel(LCDIF_CTRL_RUN, &regs->hw_lcdif_ctrl_set);
 }
 
+static int mxs_probe_common(struct ctfb_res_modes *mode, int bpp, void *fb)
+{
+       /* Start framebuffer */
+       mxs_lcd_init((u32)fb, mode, bpp);
+
+#ifdef CONFIG_VIDEO_MXS_MODE_SYSTEM
+       /*
+        * If the LCD runs in system mode, the LCD refresh has to be triggered
+        * manually by setting the RUN bit in HW_LCDIF_CTRL register. To avoid
+        * having to set this bit manually after every single change in the
+        * framebuffer memory, we set up specially crafted circular DMA, which
+        * sets the RUN bit, then waits until it gets cleared and repeats this
+        * infinitelly. This way, we get smooth continuous updates of the LCD.
+        */
+       struct mxs_lcdif_regs *regs = (struct mxs_lcdif_regs *)MXS_LCDIF_BASE;
+
+       memset(&desc, 0, sizeof(struct mxs_dma_desc));
+       desc.address = (dma_addr_t)&desc;
+       desc.cmd.data = MXS_DMA_DESC_COMMAND_NO_DMAXFER | MXS_DMA_DESC_CHAIN |
+                       MXS_DMA_DESC_WAIT4END |
+                       (1 << MXS_DMA_DESC_PIO_WORDS_OFFSET);
+       desc.cmd.pio_words[0] = readl(&regs->hw_lcdif_ctrl) | LCDIF_CTRL_RUN;
+       desc.cmd.next = (uint32_t)&desc.cmd;
+
+       /* Execute the DMA chain. */
+       mxs_dma_circ_start(MXS_DMA_CHANNEL_AHB_APBH_LCDIF, &desc);
+#endif
+
+       return 0;
+}
+
 void lcdif_power_down(void)
 {
        struct mxs_lcdif_regs *regs = (struct mxs_lcdif_regs *)MXS_LCDIF_BASE;
@@ -151,8 +182,9 @@ void lcdif_power_down(void)
 void *video_hw_init(void)
 {
        int bpp = -1;
+       int ret = 0;
        char *penv;
-       void *fb;
+       void *fb = NULL;
        struct ctfb_res_modes mode;
 
        puts("Video: ");
@@ -167,8 +199,7 @@ void *video_hw_init(void)
        bpp = video_get_params(&mode, penv);
 
        /* fill in Graphic device struct */
-       sprintf(panel.modeIdent, "%dx%dx%d",
-                       mode.xres, mode.yres, bpp);
+       sprintf(panel.modeIdent, "%dx%dx%d", mode.xres, mode.yres, bpp);
 
        panel.winSizeX = mode.xres;
        panel.winSizeY = mode.yres;
@@ -211,31 +242,14 @@ void *video_hw_init(void)
 
        printf("%s\n", panel.modeIdent);
 
-       /* Start framebuffer */
-       mxs_lcd_init(panel.frameAdrs, &mode, bpp);
-
-#ifdef CONFIG_VIDEO_MXS_MODE_SYSTEM
-       /*
-        * If the LCD runs in system mode, the LCD refresh has to be triggered
-        * manually by setting the RUN bit in HW_LCDIF_CTRL register. To avoid
-        * having to set this bit manually after every single change in the
-        * framebuffer memory, we set up specially crafted circular DMA, which
-        * sets the RUN bit, then waits until it gets cleared and repeats this
-        * infinitelly. This way, we get smooth continuous updates of the LCD.
-        */
-       struct mxs_lcdif_regs *regs = (struct mxs_lcdif_regs *)MXS_LCDIF_BASE;
+       ret = mxs_probe_common(&mode, bpp, fb);
+       if (ret)
+               goto dealloc_fb;
 
-       memset(&desc, 0, sizeof(struct mxs_dma_desc));
-       desc.address = (dma_addr_t)&desc;
-       desc.cmd.data = MXS_DMA_DESC_COMMAND_NO_DMAXFER | MXS_DMA_DESC_CHAIN |
-                       MXS_DMA_DESC_WAIT4END |
-                       (1 << MXS_DMA_DESC_PIO_WORDS_OFFSET);
-       desc.cmd.pio_words[0] = readl(&regs->hw_lcdif_ctrl) | LCDIF_CTRL_RUN;
-       desc.cmd.next = (uint32_t)&desc.cmd;
+       return (void *)&panel;
 
-       /* Execute the DMA chain. */
-       mxs_dma_circ_start(MXS_DMA_CHANNEL_AHB_APBH_LCDIF, &desc);
-#endif
+dealloc_fb:
+       free(fb);
 
-       return (void *)&panel;
+       return NULL;
 }