rk_fb: update uboot & kernel logo parse

Sometimes we want to display logo at hdmi screen. but hdmi uboot
resolution maybe different with framebuffer size, so we need read
logo config from regs and decide how to display logo at kernel.

now only support uboot logo size = kernel logo size

Signed-off-by: Mark Yao <mark.yao@rock-chips.com>

drivers/video/rockchip/lcdc/rk312x_lcdc.c

@@ -1057,6 +1057,52 @@ static void rk312x_lcdc_select_bcsh(struct rk_lcdc_driver *dev_drv,
 		}
 }
 
+static int rk312x_get_dspbuf_info(struct rk_lcdc_driver *dev_drv, u16 *xact,
+				  u16 *yact, int *format, u32 *dsp_addr)
+{
+	struct lcdc_device *lcdc_dev = container_of(dev_drv,
+						    struct lcdc_device, driver);
+	u32 val;
+
+	spin_lock(&lcdc_dev->reg_lock);
+
+	val = lcdc_readl(lcdc_dev, WIN0_ACT_INFO);
+	*xact = (val & m_ACT_WIDTH)+1;
+	*yact = ((val & m_ACT_HEIGHT)>>16)+1;
+
+	val = lcdc_readl(lcdc_dev, SYS_CTRL);
+
+	*format = (val & m_WIN0_FORMAT) >> 3;
+	*dsp_addr = lcdc_readl(lcdc_dev, WIN0_YRGB_MST);
+
+	spin_unlock(&lcdc_dev->reg_lock);
+
+	return 0;
+}
+
+static int rk312x_post_dspbuf(struct rk_lcdc_driver *dev_drv, u32 rgb_mst,
+			      int format, u16 xact, u16 yact, u16 xvir)
+{
+	struct lcdc_device *lcdc_dev = container_of(dev_drv,
+						    struct lcdc_device, driver);
+	u32 val, mask;
+
+	mask = m_WIN0_FORMAT;
+	val = v_WIN0_FORMAT(format);
+	lcdc_msk_reg(lcdc_dev, SYS_CTRL, mask, val);
+
+	lcdc_msk_reg(lcdc_dev, WIN0_VIR, m_YRGB_VIR,
+			v_YRGB_VIR(xvir));
+	lcdc_writel(lcdc_dev, WIN0_ACT_INFO, v_ACT_WIDTH(xact) |
+		    v_ACT_HEIGHT(yact));
+
+	lcdc_writel(lcdc_dev, WIN0_YRGB_MST, rgb_mst);
+
+	lcdc_cfg_done(lcdc_dev);
+
+	return 0;
+}
+
 static int rk312x_load_screen(struct rk_lcdc_driver *dev_drv, bool initscreen)
 {
 	u16 face = 0;
@@ -2395,6 +2441,8 @@ static int rk312x_lcdc_dsp_black(struct rk_lcdc_driver *dev_drv, int enable)
 static struct rk_lcdc_drv_ops lcdc_drv_ops = {
 	.open = rk312x_lcdc_open,
 	.load_screen = rk312x_load_screen,
+	.get_dspbuf_info = rk312x_get_dspbuf_info,
+	.post_dspbuf = rk312x_post_dspbuf,
 	.set_par = rk312x_lcdc_set_par,
 	.pan_display = rk312x_lcdc_pan_display,
 	.direct_set_addr = rk312x_lcdc_direct_set_win_addr,

drivers/video/rockchip/lcdc/rk3288_lcdc.c

@@ -1063,6 +1063,52 @@ static int rk3288_lcdc_set_dclk(struct rk_lcdc_driver *dev_drv)
 
 }
 
+static int rk3288_get_dspbuf_info(struct rk_lcdc_driver *dev_drv, u16 *xact,
+				  u16 *yact, int *format, u32 *dsp_addr)
+{
+	struct lcdc_device *lcdc_dev = container_of(dev_drv,
+						    struct lcdc_device, driver);
+	u32 val;
+
+	spin_lock(&lcdc_dev->reg_lock);
+
+	val = lcdc_readl(lcdc_dev, WIN0_ACT_INFO);
+	*xact = (val & m_WIN0_ACT_WIDTH) + 1;
+	*yact = ((val & m_WIN0_ACT_HEIGHT)>>16) + 1;
+
+	val = lcdc_readl(lcdc_dev, WIN0_CTRL0);
+	*format = (val & m_WIN0_DATA_FMT) >> 1;
+	*dsp_addr = lcdc_readl(lcdc_dev, WIN0_YRGB_MST);
+
+	spin_unlock(&lcdc_dev->reg_lock);
+
+	return 0;
+}
+
+static int rk3288_post_dspbuf(struct rk_lcdc_driver *dev_drv, u32 rgb_mst,
+			      int format, u16 xact, u16 yact, u16 xvir)
+{
+	struct lcdc_device *lcdc_dev = container_of(dev_drv,
+						    struct lcdc_device, driver);
+	u32 val, mask;
+	int swap = (format == RGB888) ? 1 : 0;
+
+	mask = m_WIN0_DATA_FMT | m_WIN0_RB_SWAP;
+	val = v_WIN0_DATA_FMT(format) | v_WIN0_RB_SWAP(swap);
+	lcdc_msk_reg(lcdc_dev, WIN0_CTRL0, mask, val);
+
+	lcdc_msk_reg(lcdc_dev, WIN0_VIR, m_WIN0_VIR_STRIDE,
+			v_WIN0_VIR_STRIDE(xvir));
+	lcdc_writel(lcdc_dev, WIN0_ACT_INFO, v_WIN0_ACT_WIDTH(xact) |
+		    v_WIN0_ACT_HEIGHT(yact));
+
+	lcdc_writel(lcdc_dev, WIN0_YRGB_MST, rgb_mst);
+
+	lcdc_cfg_done(lcdc_dev);
+
+	return 0;
+}
+
 static int rk3288_load_screen(struct rk_lcdc_driver *dev_drv, bool initscreen)
 {
 	u16 face = 0;
@@ -3535,6 +3581,8 @@ static struct rk_lcdc_drv_ops lcdc_drv_ops = {
 	.open 			= rk3288_lcdc_open,
 	.win_direct_en		= rk3288_lcdc_win_direct_en,
 	.load_screen 		= rk3288_load_screen,
+	.get_dspbuf_info	= rk3288_get_dspbuf_info,
+	.post_dspbuf		= rk3288_post_dspbuf,
 	.set_par 		= rk3288_lcdc_set_par,
 	.pan_display 		= rk3288_lcdc_pan_display,
 	.direct_set_addr 	= rk3288_lcdc_direct_set_win_addr,

drivers/video/rockchip/rk_fb.c

@@ -4151,6 +4151,9 @@ int rk_fb_register(struct rk_lcdc_driver *dev_drv,
 	/* show logo for primary display device */
 #if !defined(CONFIG_FRAMEBUFFER_CONSOLE)
 	if (dev_drv->prop == PRMRY) {
+		u16 xact, yact;
+		int format;
+		u32 dsp_addr;
 		struct fb_info *main_fbi = rk_fb->fb[0];
 		main_fbi->fbops->fb_open(main_fbi, 1);
 
@@ -4165,8 +4168,8 @@ int rk_fb_register(struct rk_lcdc_driver *dev_drv,
 		rk_fb_alloc_buffer(main_fbi, 0);	/* only alloc memory for main fb */
 		dev_drv->uboot_logo = support_uboot_display();
 
-		if (uboot_logo_offset && uboot_logo_base) {
+		if (dev_drv->uboot_logo &&
-			struct rk_lcdc_win *win = dev_drv->win[0];
+		    uboot_logo_offset && uboot_logo_base) {
 			int width, height, bits;
 			phys_addr_t start = uboot_logo_base + uboot_logo_offset;
 			unsigned int size = uboot_logo_size - uboot_logo_offset;
@@ -4175,6 +4178,9 @@ int rk_fb_register(struct rk_lcdc_driver *dev_drv,
 			char *vaddr;
 			int i = 0;
 
+			if (dev_drv->ops->get_dspbuf_info)
+				dev_drv->ops->get_dspbuf_info(dev_drv, &xact,
+					&yact, &format,	&dsp_addr);
 			nr_pages = size >> PAGE_SHIFT;
 			pages = kzalloc(sizeof(struct page) * nr_pages,
 					GFP_KERNEL);
@@ -4199,35 +4205,75 @@ int rk_fb_register(struct rk_lcdc_driver *dev_drv,
 			}
 			kfree(pages);
 			vunmap(vaddr);
-			if (width > main_fbi->var.xres ||
+			if (dev_drv->uboot_logo &&
-			    height > main_fbi->var.yres) {
+			    (width != xact || height != yact)) {
-				pr_err("ERROR: logo size out of screen range");
+				pr_err("can't support uboot kernel logo use different size [%dx%d] != [%dx%d]\n",
+					xact, yact, width, height);
 				return 0;
 			}
 
-			win->area[0].format = rk_fb_data_fmt(0, bits);
+			if (dev_drv->ops->post_dspbuf) {
-			win->area[0].y_vir_stride = width * bits >> 5;
+				dev_drv->ops->post_dspbuf(dev_drv,
-			win->area[0].xpos = (main_fbi->var.xres - width) >> 1;
+					main_fbi->fix.smem_start,
-			win->area[0].ypos = (main_fbi->var.yres - height) >> 1;
+					rk_fb_data_fmt(0, bits),
-			win->area[0].xsize = width;
+					width, height, width * bits >> 5);
-			win->area[0].ysize = height;
+			}
-			win->area[0].xact = width;
+			if (dev_drv->iommu_enabled) {
-			win->area[0].yact = height;
+				rk_fb_poll_wait_frame_complete();
-			win->area[0].xvir = win->area[0].y_vir_stride;
+				if (dev_drv->ops->mmu_en)
-			win->area[0].yvir = height;
+					dev_drv->ops->mmu_en(dev_drv);
-			win->area[0].smem_start = main_fbi->fix.smem_start;
+				freed_index = 0;
-			win->area[0].y_offset = 0;
+			}
 
-			win->area_num = 1;
+			return 0;
-			win->alpha_mode = 4;
+		} else if (dev_drv->uboot_logo && uboot_logo_base) {
-			win->alpha_en = 0;
+			phys_addr_t start = uboot_logo_base;
-			win->g_alpha_val = 0;
+			int logo_len, i=0;
+			unsigned int nr_pages;
+			struct page **pages;
+			char *vaddr;
 
-			win->state = 1;
+			dev_drv->ops->get_dspbuf_info(dev_drv, &xact,
-			dev_drv->ops->set_par(dev_drv, 0);
+					              &yact, &format,
-			dev_drv->ops->pan_display(dev_drv, 0);
+						      &start);
-			dev_drv->ops->cfg_done(dev_drv);
+			logo_len = rk_fb_pixel_width(format) * xact * yact >> 3;
+			if (logo_len > uboot_logo_size ||
+			    logo_len > main_fbi->fix.smem_len) {
+				pr_err("logo size > uboot reserve buffer size\n");
+				return -1;
+			}
 
+			nr_pages = uboot_logo_size >> PAGE_SHIFT;
+			pages = kzalloc(sizeof(struct page) * nr_pages,
+					GFP_KERNEL);
+			while (i < nr_pages) {
+				pages[i] = phys_to_page(start);
+				start += PAGE_SIZE;
+				i++;
+			}
+			vaddr = vmap(pages, nr_pages, VM_MAP,
+					pgprot_writecombine(PAGE_KERNEL));
+			if (!vaddr) {
+				pr_err("failed to vmap phy addr %x\n",
+					uboot_logo_base);
+				return -1;
+			}
+
+			memcpy(main_fbi->screen_base, vaddr, logo_len);
+
+			kfree(pages);
+			vunmap(vaddr);
+
+			dev_drv->ops->post_dspbuf(dev_drv,
+					main_fbi->fix.smem_start,
+					format,	xact, yact,
+					xact * rk_fb_pixel_width(format) >> 5);
+			if (dev_drv->iommu_enabled) {
+				rk_fb_poll_wait_frame_complete();
+				if (dev_drv->ops->mmu_en)
+					dev_drv->ops->mmu_en(dev_drv);
+				freed_index = 0;
+			}
 			return 0;
 		}
 #if defined(CONFIG_LOGO)

include/linux/rk_fb.h

@@ -428,6 +428,12 @@ struct rk_lcdc_drv_ops {
 	ssize_t(*get_disp_info) (struct rk_lcdc_driver *dev_drv, char *buf,
 				  int layer_id);
 	int (*load_screen) (struct rk_lcdc_driver *dev_drv, bool initscreen);
+	int (*get_dspbuf_info) (struct rk_lcdc_driver *dev_drv,
+				u16 *xact, u16 *yact, int *format,
+				u32 *dsp_addr);
+	int (*post_dspbuf)(struct rk_lcdc_driver *dev_drv, u32 rgb_mst,
+			   int format, u16 xact, u16 yact, u16 xvir);
+
 	int (*get_win_state) (struct rk_lcdc_driver *dev_drv, int layer_id);
 	int (*ovl_mgr) (struct rk_lcdc_driver *dev_drv, int swap, bool set);	/*overlay manager*/
 	int (*fps_mgr) (struct rk_lcdc_driver *dev_drv, int fps, bool set);