From 57df1041409b50f2cc3137a820254fa982c49c45 Mon Sep 17 00:00:00 2001
From: Andrzej Zaborowski <balrog@zabor.org>
Date: Wed, 2 Jul 2008 22:43:11 +0100
Subject: [PATCH] Subject: [PATCH] glamo: Don't disable hwcursor for blinking and use vsync-wait.

---
 drivers/mfd/glamo/glamo-fb.c |  109 +++++++++++++++++++++++++++++-------------
 1 files changed, 76 insertions(+), 33 deletions(-)

diff --git a/drivers/mfd/glamo/glamo-fb.c b/drivers/mfd/glamo/glamo-fb.c
index 16e9d2e..7feef32 100644
--- a/drivers/mfd/glamo/glamo-fb.c
+++ b/drivers/mfd/glamo/glamo-fb.c
@@ -72,6 +72,7 @@ struct glamofb_handle {
 	char __iomem *base;
 	struct glamofb_platform_data *mach_info;
 	char __iomem *cursor_addr;
+	int cursor_on;
 	u_int32_t pseudo_pal[16];
 	spinlock_t lock_cmd;
 };
@@ -497,18 +498,53 @@ static int glamofb_setcolreg(unsigned regno,
 }
 
 #ifdef CONFIG_MFD_GLAMO_HWACCEL
+static inline void glamofb_vsync_wait(struct glamofb_handle *glamo,
+		int line, int size, int range)
+{
+	int count[2];
+
+	do {
+		count[0] = reg_read(glamo, GLAMO_REG_LCD_STATUS2) & 0x3ff;
+		count[1] = reg_read(glamo, GLAMO_REG_LCD_STATUS2) & 0x3ff;
+	} while (count[0] != count[1] ||
+			(line < count[0] + range &&
+			 size > count[0] - range) ||
+			count[0] < range * 2);
+}
+
+/*
+ * Enable/disable the hardware cursor mode altogether
+ * (for blinking and such, use glamofb_cursor()).
+ */
+static void glamofb_cursor_onoff(struct glamofb_handle *glamo, int on)
+{
+	int y, size;
+
+	if (glamo->cursor_on) {
+		y = reg_read(glamo, GLAMO_REG_LCD_CURSOR_Y_POS);
+		size = reg_read(glamo, GLAMO_REG_LCD_CURSOR_Y_SIZE);
+
+		glamofb_vsync_wait(glamo, y, size, 30);
+	}
+
+	reg_set_bit_mask(glamo, GLAMO_REG_LCD_MODE1,
+			GLAMO_LCD_MODE1_CURSOR_EN,
+			on ? GLAMO_LCD_MODE1_CURSOR_EN : 0);
+	glamo->cursor_on = on;
+
+	/* Hide the cursor by default */
+	reg_write(glamo, GLAMO_REG_LCD_CURSOR_X_SIZE, 0);
+}
+
 static int glamofb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 {
 	struct glamofb_handle *glamo = info->par;
 	unsigned long flags;
 
-	if (cursor->image.depth > 2)
-		return -EINVAL;
-
 	spin_lock_irqsave(&glamo->lock_cmd, flags);
 
-	reg_set_bit_mask(glamo, GLAMO_REG_LCD_MODE1,
-			GLAMO_LCD_MODE1_CURSOR_EN, 0);
+	reg_write(glamo, GLAMO_REG_LCD_CURSOR_X_SIZE,
+			cursor->enable ? cursor->image.width : 0);
 
 	if (cursor->set & FB_CUR_SETPOS) {
 		reg_write(glamo, GLAMO_REG_LCD_CURSOR_X_POS,
@@ -518,12 +554,12 @@ static int glamofb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 	}
 
 	if (cursor->set & FB_CUR_SETCMAP) {
-		uint16_t fg = cursor->image.fg_color;
-		uint16_t bg = cursor->image.bg_color;
+		uint16_t fg = glamo->pseudo_pal[cursor->image.fg_color];
+		uint16_t bg = glamo->pseudo_pal[cursor->image.bg_color];
 
 		reg_write(glamo, GLAMO_REG_LCD_CURSOR_FG_COLOR, fg);
 		reg_write(glamo, GLAMO_REG_LCD_CURSOR_BG_COLOR, bg);
-		reg_write(glamo, GLAMO_REG_LCD_CURSOR_DST_COLOR, bg);
+		reg_write(glamo, GLAMO_REG_LCD_CURSOR_DST_COLOR, fg);
 	}
 
 	if (cursor->set & FB_CUR_SETHOT)
@@ -532,23 +568,27 @@ static int glamofb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 
 	if ((cursor->set & FB_CUR_SETSIZE) ||
 	    (cursor->set & (FB_CUR_SETIMAGE | FB_CUR_SETSHAPE))) {
-		int x, y, pitch;
-		const unsigned char *pcol = cursor->image.data;
-		const unsigned char *pmsk = cursor->mask;
-		void __iomem *dst = glamo->cursor_addr;
-		unsigned char dcol = 0;
-		unsigned char dmsk = 0;
-		unsigned char byte = 0;
-
-		pitch = (cursor->image.width + 3) >> 2;
-		reg_write(glamo, GLAMO_REG_LCD_CURSOR_X_SIZE,
-			  cursor->image.width);
+		int x, y, pitch, op;
+		const uint8_t *pcol = cursor->image.data;
+		const uint8_t *pmsk = cursor->mask;
+		uint8_t __iomem *dst = glamo->cursor_addr;
+		uint8_t dcol = 0;
+		uint8_t dmsk = 0;
+		uint8_t byte = 0;
+
+		if (cursor->image.depth > 1) {
+			spin_unlock_irqrestore(&glamo->lock_cmd, flags);
+			return -EINVAL;
+		}
+
+		pitch = ((cursor->image.width + 7) >> 2) & ~1;
 		reg_write(glamo, GLAMO_REG_LCD_CURSOR_PITCH,
-			  pitch);
+			pitch);
 		reg_write(glamo, GLAMO_REG_LCD_CURSOR_Y_SIZE,
-			  cursor->image.height);
+			cursor->image.height);
 
 		for (y = 0; y < cursor->image.height; y++) {
+			byte = 0;
 			for (x = 0; x < cursor->image.width; x++) {
 				if ((x % 8) == 0) {
 					dcol = *pcol++;
@@ -558,28 +598,28 @@ static int glamofb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 					dmsk >>= 1;
 				}
 
-				if (dmsk & 1) {
-					unsigned int op;
+				if (cursor->rop == ROP_COPY)
+					op = (dmsk & 1) ?
+						(dcol & 1) ? 1 : 3 : 0;
+				else
+					op = ((dmsk & 1) << 1) |
+						((dcol & 1) << 0);
+				byte |= op << ((x & 3) << 1);
 
-					op = (dcol & 1) ? 1 : 3;
-					byte |= op << ((x % 4) * 2);
-				}
-
-				if ((x % 4) == 0) {
+				if (x % 4 == 3) {
 					writeb(byte, dst + x / 4);
 					byte = 0;
 				}
 			}
+			if (x % 4) {
+				writeb(byte, dst + x / 4);
+				byte = 0;
+			}
 
 			dst += pitch;
 		}
 	}
 
-	if (cursor->enable)
-		reg_set_bit_mask(glamo, GLAMO_REG_LCD_MODE1,
-				GLAMO_LCD_MODE1_CURSOR_EN,
-				GLAMO_LCD_MODE1_CURSOR_EN);
-
 	spin_unlock_irqrestore(&glamo->lock_cmd, flags);
 
 	return 0;
@@ -806,6 +846,9 @@ static int __init glamofb_probe(struct platform_device *pdev)
 
 	spin_lock_init(&glamofb->lock_cmd);
 	glamofb_init_regs(glamofb);
+#ifdef CONFIG_MFD_GLAMO_HWACCEL
+	glamofb_cursor_onoff(glamofb, 1);
+#endif
 
 	rc = register_framebuffer(fbinfo);
 	if (rc < 0) {
-- 
1.5.6.5