3 files changed, 33 insertions, 17 deletions

diff --git a/drivers/video/omap2/omapfb/omapfb-ioctl.c b/drivers/video/omap2/omapfb/omapfb-ioctl.c
index d6ec56d..4aacd01 100644
--- a/drivers/video/omap2/omapfb/omapfb-ioctl.c
+++ b/drivers/video/omap2/omapfb/omapfb-ioctl.c
@@ -917,9 +917,11 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 
 		if (display->state == OMAP_DSS_DISPLAY_ACTIVE) {
 			if (p.crt)
-				omapfb_enable_vsync(fbdev);
+				omapfb_enable_vsync(fbdev, display->channel,
+					true);
 			else
-				omapfb_disable_vsync(fbdev);
+				omapfb_enable_vsync(fbdev, display->channel,
+					false);
 		}
 		omapfb_unlock(fbdev);
 		break;
diff --git a/drivers/video/omap2/omapfb/omapfb-main.c b/drivers/video/omap2/omapfb/omapfb-main.c
index 6cfa69e..46752ab 100644
--- a/drivers/video/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/omap2/omapfb/omapfb-main.c
@@ -837,8 +837,9 @@ static int omapfb_release(struct fb_info *fbi, int user)
 {
 	struct omapfb_info *ofbi = FB2OFB(fbi);
 	struct omapfb2_device *fbdev = ofbi->fbdev;
+	struct omap_dss_device *display = fb2display(fbi);
 
-	omapfb_disable_vsync(fbdev);
+	omapfb_enable_vsync(fbdev, display->channel, false);
 
 	return 0;
 }
@@ -1398,8 +1399,9 @@ static int omapfb_blank(int blank, struct fb_info *fbi)
 				r = display->driver->enable(display);
 		}
 
-		if (fbdev->vsync_active)
-			omapfb_enable_vsync(fbdev);
+		if (fbdev->vsync_active &&
+			(display->state == OMAP_DSS_DISPLAY_ACTIVE))
+			omapfb_enable_vsync(fbdev, display->channel, true);
 
 		break;
 
@@ -1411,7 +1413,7 @@ static int omapfb_blank(int blank, struct fb_info *fbi)
 	case FB_BLANK_POWERDOWN:
 
 		if (fbdev->vsync_active)
-			omapfb_disable_vsync(fbdev);
+			omapfb_enable_vsync(fbdev, display->channel, false);
 
 		if (display->state != OMAP_DSS_DISPLAY_ACTIVE)
 			goto exit;
@@ -2402,17 +2404,29 @@ static void omapfb_vsync_isr(void *data, u32 mask)
 	schedule_work(&fbdev->vsync_work);
 }
 
-int omapfb_enable_vsync(struct omapfb2_device *fbdev)
+int omapfb_enable_vsync(struct omapfb2_device *fbdev, enum omap_channel ch,
+	bool enable)
 {
-	int r;
-	/* TODO: should determine correct IRQ like dss_mgr_wait_for_vsync does*/
-	r = omap_dispc_register_isr(omapfb_vsync_isr, fbdev, DISPC_IRQ_VSYNC);
-	return r;
-}
+	int r = 0;
+	const u32 masks[] = {
+		DISPC_IRQ_VSYNC,
+		DISPC_IRQ_EVSYNC_EVEN,
+		DISPC_IRQ_VSYNC2
+	};
+
+	if (ch > OMAP_DSS_CHANNEL_LCD2) {
+		pr_warn("%s wrong channel number\n", __func__);
+		return -ENODEV;
+	}
 
-void omapfb_disable_vsync(struct omapfb2_device *fbdev)
-{
-	omap_dispc_unregister_isr(omapfb_vsync_isr, fbdev, DISPC_IRQ_VSYNC);
+	if (enable)
+		r = omap_dispc_register_isr(omapfb_vsync_isr, fbdev,
+			masks[ch]);
+	else
+		r = omap_dispc_unregister_isr(omapfb_vsync_isr, fbdev,
+			masks[ch]);
+
+	return r;
 }
 
 static int omapfb_probe(struct platform_device *pdev)
diff --git a/drivers/video/omap2/omapfb/omapfb.h b/drivers/video/omap2/omapfb/omapfb.h
index 5c67365..9314a8e 100644
--- a/drivers/video/omap2/omapfb/omapfb.h
+++ b/drivers/video/omap2/omapfb/omapfb.h
@@ -132,8 +132,8 @@ int dss_mode_to_fb_mode(enum omap_color_mode dssmode,
 int omapfb_setup_overlay(struct fb_info *fbi, struct omap_overlay *ovl,
 		u16 posx, u16 posy, u16 outw, u16 outh);
 
-int omapfb_enable_vsync(struct omapfb2_device *fbdev);
-void omapfb_disable_vsync(struct omapfb2_device *fbdev);
+int omapfb_enable_vsync(struct omapfb2_device *fbdev, enum omap_channel ch,
+			bool enable);
 void get_fb_resolution(struct omap_dss_device *dssdev,
 		u16 *xres, u16 *yres);