Add proper VT switching. Save and restore hardware registers. This should fix

suspend/resume.

6 files changed, 266 insertions, 142 deletions

diff --git a/src/glamo-cmdq.c b/src/glamo-cmdq.c
index 5b3abbd..c690407 100644
--- a/src/glamo-cmdq.c
+++ b/src/glamo-cmdq.c
@@ -36,9 +36,6 @@
 static void
 GLAMOCMDQResetCP(GlamoPtr pGlamo);
 
-static void
-GLAMODumpRegs(GlamoPtr pGlamo, CARD16 from, CARD16 to);
-
 #define CQ_LEN 255
 #define CQ_MASK ((CQ_LEN + 1) * 1024 - 1)
 #define CQ_MASKL (CQ_MASK & 0xffff)
@@ -46,6 +43,9 @@ GLAMODumpRegs(GlamoPtr pGlamo, CARD16 from, CARD16 to);
 
 #if 0
 static void
+GLAMODumpRegs(GlamoPtr pGlamo, CARD16 from, CARD16 to);
+
+static void
 GLAMODebugFifo(GlamoPtr pGlamo)
 {
 	GLAMOCardInfo *glamoc = pGlamo->glamoc;
@@ -291,26 +291,6 @@ GLAMOEngineWait(GlamoPtr pGlamo,
 	GLAMOEngineWaitReal(pGlamo, engine, TRUE);
 }
 
-MemBuf *
-GLAMOCreateCMDQCache(GlamoPtr pGlamo)
-{
-	MemBuf *buf;
-
-	buf = (MemBuf *)xcalloc(1, sizeof(MemBuf));
-	if (buf == NULL)
-		return NULL;
-
-	buf->size = pGlamo->ring_len;
-	buf->address = xcalloc(1, buf->size);
-	if (buf->address == NULL) {
-		xfree(buf);
-		return NULL;
-	}
-	buf->used = 0;
-
-	return buf;
-}
-
 static void
 GLAMODispatchCMDQCache(GlamoPtr pGlamo)
 {
@@ -326,7 +306,7 @@ GLAMODispatchCMDQCache(GlamoPtr pGlamo)
     if (!buf->used)
         return;
 
-    addr = ((char *)buf->address);
+    addr = buf->data;
 	count = buf->used;
 	ring_count = pGlamo->ring_len;
 
@@ -356,7 +336,7 @@ GLAMODispatchCMDQCache(GlamoPtr pGlamo)
         /* ring_write being 0 will result in a deadlock because the cmdq read
          * will never stop. To avoid such an behaviour insert an empty
          * instruction. */
-        if(new_ring_write == 0) {
+        if (new_ring_write == 0) {
             memset((char*)(pGlamo->ring_addr), 0, 4);
             new_ring_write = 4;
         }
@@ -365,7 +345,7 @@ GLAMODispatchCMDQCache(GlamoPtr pGlamo)
         GLAMOEngineWaitReal(pGlamo, GLAMO_ENGINE_CMDQ, FALSE);
 
         /* The write position has to change to trigger a read */
-        if(ring_write == new_ring_write) {
+        if (ring_write == new_ring_write) {
             memset((char*)(pGlamo->ring_addr + new_ring_write), 0, 4);
             new_ring_write += 4;
 /*            MMIO_OUT16(mmio, GLAMO_REG_CMDQ_WRITE_ADDRH,
@@ -404,21 +384,18 @@ static void
 GLAMOCMDQResetCP(GlamoPtr pGlamo)
 {
 	volatile char *mmio = pGlamo->reg_base;
-	int cq_len = CQ_LEN;
-	CARD32 queue_offset = 0;
+	CARD32 queue_offset = pGlamo->exa_cmd_queue->offset;
 
 	/* make the decoder happy? */
 	memset((char*)pGlamo->ring_addr, 0, pGlamo->ring_len);
 
 	GLAMOEngineReset(pGlamo, GLAMO_ENGINE_CMDQ);
 
-	queue_offset = pGlamo->exa_cmd_queue->offset;
-
 	MMIO_OUT16(mmio, GLAMO_REG_CMDQ_BASE_ADDRL,
 		   queue_offset & 0xffff);
 	MMIO_OUT16(mmio, GLAMO_REG_CMDQ_BASE_ADDRH,
 		   (queue_offset >> 16) & 0x7f);
-	MMIO_OUT16(mmio, GLAMO_REG_CMDQ_LEN, cq_len);
+	MMIO_OUT16(mmio, GLAMO_REG_CMDQ_LEN, CQ_LEN);
 
 	MMIO_OUT16(mmio, GLAMO_REG_CMDQ_WRITE_ADDRH, 0);
 	MMIO_OUT16(mmio, GLAMO_REG_CMDQ_WRITE_ADDRL, 0);
@@ -431,58 +408,76 @@ GLAMOCMDQResetCP(GlamoPtr pGlamo)
 	GLAMOEngineWaitReal(pGlamo, GLAMO_ENGINE_ALL, FALSE);
 }
 
-static Bool
-GLAMOCMDQInit(GlamoPtr pGlamo,
-	      Bool force)
+Bool
+GLAMOCMDQInit(ScrnInfoPtr pScrn)
 {
-	int cq_len = CQ_LEN;
+    GlamoPtr pGlamo = GlamoPTR(pScrn);
+    MemBuf *buf;
 
-	if (!force && pGlamo->exa_cmd_queue)
-		return TRUE;
+    pGlamo->ring_len = (CQ_LEN + 1) * 1024;
 
-	pGlamo->ring_len = (cq_len + 1) * 1024;
+    buf = (MemBuf *)xcalloc(1, sizeof(MemBuf) + pGlamo->ring_len - 1);
 
-	pGlamo->exa_cmd_queue =
-		exaOffscreenAlloc(pGlamo->pScreen, pGlamo->ring_len,
-				  pGlamo->exa->pixmapOffsetAlign,
-				  TRUE, NULL, NULL);
+    if (!buf) {
+        return FALSE;
+    }
 
-	if (!pGlamo->exa_cmd_queue)
-		return FALSE;
-	pGlamo->ring_addr =
-		(CARD16 *) (pGlamo->fbstart +
-				pGlamo->exa_cmd_queue->offset);
+	buf->size = pGlamo->ring_len;
+	buf->used = 0;
 
-	GLAMOEngineEnable(pGlamo, GLAMO_ENGINE_CMDQ);
+    pGlamo->exa_cmd_queue = exaOffscreenAlloc(pGlamo->pScreen, pGlamo->ring_len,
+				                              pGlamo->exa->pixmapOffsetAlign,
+                                              TRUE, NULL, NULL);
 
-	GLAMOCMDQResetCP(pGlamo);
+    if (!pGlamo->exa_cmd_queue) {
+        xfree(buf);
+        return FALSE;
+    }
+
+    pGlamo->ring_addr = (CARD16 *)(pGlamo->fbstart + pGlamo->exa_cmd_queue->offset);
+
+	pGlamo->cmd_queue_cache = buf;
 
-	return TRUE;
+    return TRUE;
 }
 
-void
-GLAMOCMDQCacheSetup(GlamoPtr pGlamo)
-{
-	GLAMOCMDQInit(pGlamo, TRUE);
-	if (pGlamo->cmd_queue_cache)
-		return;
-	pGlamo->cmd_queue_cache = GLAMOCreateCMDQCache(pGlamo);
-	if (pGlamo->cmd_queue_cache == FALSE)
-		FatalError("Failed to allocate cmd queue cache buffer.\n");
+Bool
+GLAMOCMDQEnable(ScrnInfoPtr pScrn) {
+    GlamoPtr pGlamo = GlamoPTR(pScrn);
+
+    pGlamo->ring_addr =	(CARD16 *) (pGlamo->fbstart + pGlamo->exa_cmd_queue->offset);
+
+    GLAMOEngineEnable(pGlamo, GLAMO_ENGINE_CMDQ);
+    GLAMOCMDQResetCP(pGlamo);
+
+    return TRUE;
 }
 
 void
-GLAMOCMQCacheTeardown(GlamoPtr pGlamo)
-{
+GLAMOCMDQDisable(ScrnInfoPtr pScrn) {
+    GlamoPtr pGlamo = GlamoPTR(pScrn);
+
 	GLAMOEngineWait(pGlamo, GLAMO_ENGINE_ALL);
+    GLAMOEngineDisable(pGlamo, GLAMO_ENGINE_CMDQ);
+}
+
+void
+GLAMOCMDQFini(ScrnInfoPtr pScrn) {
+    GlamoPtr pGlamo = GlamoPTR(pScrn);
+
+    GLAMOCMDQDisable(pScrn);
 
-	xfree(pGlamo->cmd_queue_cache->address);
-	xfree(pGlamo->cmd_queue_cache);
-	pGlamo->cmd_queue_cache = NULL;
-    if(0)
-        GLAMODumpRegs(pGlamo, 0, 0);
+    if (pGlamo->cmd_queue_cache) {
+	    xfree(pGlamo->cmd_queue_cache);
+	    pGlamo->cmd_queue_cache = NULL;
+    }
+    if (pGlamo->exa_cmd_queue) {
+        exaOffscreenFree(pGlamo->pScreen, pGlamo->exa_cmd_queue);
+        pGlamo->exa_cmd_queue = NULL;
+    }
 }
 
+#if 0
 static void
 GLAMODumpRegs(GlamoPtr pGlamo,
               CARD16 from,
@@ -495,3 +490,4 @@ GLAMODumpRegs(GlamoPtr pGlamo,
 		*(VOL16*)(pGlamo->reg_base+i));
 	}
 }
+#endif
diff --git a/src/glamo-cmdq.h b/src/glamo-cmdq.h
index 725258c..ae2db7d 100644
--- a/src/glamo-cmdq.h
+++ b/src/glamo-cmdq.h
@@ -37,7 +37,7 @@ do {									\
 	    pGlamo->cmd_queue_cache->size) {				\
 		GLAMOFlushCMDQCache(pGlamo, 1);				\
 	}								\
-	__head = (CARD16 *)((char *)pGlamo->cmd_queue_cache->address +	\
+	__head = (CARD16 *)((char *)pGlamo->cmd_queue_cache->data +	\
 	    pGlamo->cmd_queue_cache->used);				\
 	__count = 0;							\
 } while (0)
@@ -64,7 +64,7 @@ do {									\
 	    pGlamo->cmd_queue_cache->size) {				\
 		GLAMOFlushCMDQCache(pGlamo, 1);				\
 	}								\
-	__head = (CARD16 *)((char *)pGlamo->cmd_queue_cache->address +	\
+	__head = (CARD16 *)((char *)pGlamo->cmd_queue_cache->data +	\
 	    pGlamo->cmd_queue_cache->used);				\
 	__count = 0;							\
 	__total = n;							\
@@ -131,17 +131,21 @@ tv_le(struct timeval *tv1, struct timeval *tv2)
 
 #define TIMEDOUT()	(!tv_le(&_curtime, &_target))
 
-MemBuf *
-GLAMOCreateCMDQCache(GlamoPtr pGlamo);
 
 void
 GLAMOFlushCMDQCache(GlamoPtr pGlamo, Bool discard);
 
+Bool
+GLAMOCMDQInit(ScrnInfoPtr pScrn);
+
+Bool
+GLAMOCMDQEnable(ScrnInfoPtr pScrn);
+
 void
-GLAMOCMDQCacheSetup(GlamoPtr pGlamo);
+GLAMOCMDQDisable(ScrnInfoPtr pScrn);
 
 void
-GLAMOCMQCacheTeardown(GlamoPtr pGlamo);
+GLAMOCMDQFini(ScrnInfoPtr pScrn);
 
 enum GLAMOEngine {
 	GLAMO_ENGINE_CMDQ,
diff --git a/src/glamo-draw.c b/src/glamo-draw.c
index b8613f8..2a951b1 100644
--- a/src/glamo-draw.c
+++ b/src/glamo-draw.c
@@ -74,6 +74,9 @@ static const CARD8 GLAMOBltRop[16] = {
  * exa entry points declarations
  ********************************/
 
+static Bool
+GLAMODrawExaInit(ScrnInfoPtr pScrn);
+
 Bool
 GLAMOExaPrepareSolid(PixmapPtr      pPixmap,
 		     int            alu,
@@ -168,25 +171,57 @@ GLAMOWakeupHandler(pointer blockData, int result, pointer readmask)
 {
 }
 
+Bool
+GLAMODrawInit(ScrnInfoPtr pScrn)
+{
+    if (!GLAMODrawExaInit(pScrn))
+        return FALSE;
+
+	if (!GLAMOCMDQInit(pScrn)) {
+        GLAMODrawFini(pScrn);
+        return FALSE;
+    }
+    return TRUE;
+}
+
 void
-GLAMODrawSetup(GlamoPtr pGlamo)
+GLAMODrawFini(ScrnInfoPtr pScrn) {
+    GlamoPtr pGlamo = GlamoPTR(pScrn);
+
+    GLAMOCMDQFini(pScrn);
+    if (pGlamo->exa) {
+        exaDriverFini(pGlamo->pScreen);
+        xfree(pGlamo->exa);
+        pGlamo->exa = NULL;
+    }
+}
+
+Bool
+GLAMODrawEnable(ScrnInfoPtr pScrn)
 {
+    GlamoPtr pGlamo = GlamoPTR(pScrn);
+
 	GLAMOEngineEnable(pGlamo, GLAMO_ENGINE_2D);
 	GLAMOEngineReset(pGlamo, GLAMO_ENGINE_2D);
+    GLAMOCMDQEnable(pScrn);
+
+	GLAMOEngineWait(pGlamo, GLAMO_ENGINE_ALL);
+
+    return TRUE;
 }
 
 void
-GLAMODrawEnable(GlamoPtr pGlamo)
-{
-	GLAMOCMDQCacheSetup(pGlamo);
-	GLAMODrawSetup(pGlamo);
-	GLAMOEngineWait(pGlamo, GLAMO_ENGINE_ALL);
+GLAMODrawDisable(ScrnInfoPtr pScrn) {
+    GlamoPtr pGlamo = GlamoPTR(pScrn);
+
+    GLAMOCMDQDisable(pScrn);
+	GLAMOEngineDisable(pGlamo, GLAMO_ENGINE_2D);
 }
 
-Bool
-GLAMODrawExaInit(ScreenPtr pScreen, ScrnInfoPtr pScrn)
+static Bool
+GLAMODrawExaInit(ScrnInfoPtr pScrn)
 {
-	GlamoPtr pGlamo = GlamoPTR(pScrn);
+    GlamoPtr pGlamo = GlamoPTR(pScrn);
 
 	Bool success = FALSE;
 	ExaDriverPtr exa;
@@ -195,15 +230,16 @@ GLAMODrawExaInit(ScreenPtr pScreen, ScrnInfoPtr pScrn)
 			"EXA hardware acceleration initialising\n");
 
 	exa = pGlamo->exa = exaDriverAlloc();
-    if(!exa) return FALSE;
+    if(!exa)
+        return FALSE;
 
 	exa->memoryBase = pGlamo->fbstart;
 	exa->memorySize = 1024 * 1024 * 4;
 	/*exa->offScreenBase = pGlamo->fboff;*/
-	exa->offScreenBase = pScrn->virtualX * pScrn->virtualY * 2;
+	exa->offScreenBase = 480 * 640 * 2;
 
-	exa->exa_major = 2;
-	exa->exa_minor = 0;
+	exa->exa_major = EXA_VERSION_MAJOR;
+	exa->exa_minor = EXA_VERSION_MINOR;
 
 	exa->PrepareSolid = GLAMOExaPrepareSolid;
 	exa->Solid = GLAMOExaSolid;
@@ -218,7 +254,6 @@ GLAMODrawExaInit(ScreenPtr pScreen, ScrnInfoPtr pScrn)
 	exa->Composite = GLAMOExaComposite;
 	exa->DoneComposite = GLAMOExaDoneComposite;
 
-
 	exa->DownloadFromScreen = GLAMOExaDownloadFromScreen;
 	exa->UploadToScreen = GLAMOExaUploadToScreen;
 
@@ -235,13 +270,15 @@ GLAMODrawExaInit(ScreenPtr pScreen, ScrnInfoPtr pScrn)
 
 	RegisterBlockAndWakeupHandlers(GLAMOBlockHandler,
 				       GLAMOWakeupHandler,
-				       pScreen);
+				       pGlamo->pScreen);
 
-	success = exaDriverInit(pScreen, exa);
+	success = exaDriverInit(pGlamo->pScreen, exa);
 	if (success) {
 		ErrorF("Initialized EXA acceleration\n");
 	} else {
 		ErrorF("Failed to initialize EXA acceleration\n");
+        xfree(pGlamo->exa);
+        pGlamo->exa = NULL;
 	}
 
 	return success;
@@ -513,3 +550,4 @@ GLAMOExaWaitMarker (ScreenPtr pScreen, int marker)
 
 	GLAMOEngineWait(pGlamo, GLAMO_ENGINE_ALL);
 }
+
diff --git a/src/glamo-driver.c b/src/glamo-driver.c
index 5b9ab94..0c10acc 100644
--- a/src/glamo-driver.c
+++ b/src/glamo-driver.c
@@ -36,6 +36,7 @@
 #include "xf86RandR12.h"
 
 #include "glamo.h"
+#include "glamo-regs.h"
 
 #include <fcntl.h>
 #include <unistd.h>
@@ -76,6 +77,19 @@ GlamoCrtcResize(ScrnInfoPtr scrn, int width, int height);
 
 static Bool
 GlamoInitFramebufferDevice(ScrnInfoPtr scrn, const char *fb_device);
+
+static void
+GlamoSaveHW(ScrnInfoPtr pScrn);
+
+static void
+GlamoRestoreHW(ScrnInfoPtr pScren);
+
+static Bool
+GlamoEnterVT(int scrnIndex, int flags);
+
+static void
+GlamoLeaveVT(int scrnIndex, int flags);
+
 /* -------------------------------------------------------------------- */
 
 static const xf86CrtcConfigFuncsRec glamo_crtc_config_funcs = {
@@ -158,10 +172,6 @@ static const char *fbdevHWSymbols[] = {
 
 	/* ScrnInfo hooks */
 	"fbdevHWAdjustFrameWeak",
-	"fbdevHWEnterVTWeak",
-	"fbdevHWLeaveVTWeak",
-	"fbdevHWRestore",
-	"fbdevHWSave",
 	"fbdevHWSaveScreen",
 	"fbdevHWSaveScreenWeak",
 	"fbdevHWValidModeWeak",
@@ -311,8 +321,8 @@ GlamoProbe(DriverPtr drv, int flags)
 				pScrn->ScreenInit    = GlamoScreenInit;
 				pScrn->SwitchMode    = GlamoSwitchMode;
 				pScrn->AdjustFrame   = fbdevHWAdjustFrameWeak();
-				pScrn->EnterVT       = fbdevHWEnterVTWeak();
-				pScrn->LeaveVT       = fbdevHWLeaveVTWeak();
+				pScrn->EnterVT       = GlamoEnterVT;
+				pScrn->LeaveVT       = GlamoLeaveVT;
 				pScrn->ValidMode     = fbdevHWValidModeWeak();
 
 				xf86DrvMsg(pScrn->scrnIndex, X_INFO,
@@ -348,6 +358,8 @@ GlamoPreInit(ScrnInfoPtr pScrn, int flags)
     GlamoGetRec(pScrn);
     pGlamo = GlamoPTR(pScrn);
 
+    pGlamo->accel = FALSE;
+
     pGlamo->pEnt = xf86GetEntityInfo(pScrn->entityList[0]);
 
     pScrn->racMemFlags = RAC_FB | RAC_COLORMAP | RAC_CURSOR | RAC_VIEWPORT;
@@ -464,11 +476,10 @@ GlamoScreenInit(int scrnIndex, ScreenPtr pScreen, int argc, char **argv)
         xf86DrvMsg(scrnIndex, X_ERROR, "mapping of video memory failed\n");
         return FALSE;
     }
+
     pGlamo->fboff = fbdevHWLinearOffset(pScrn);
 
-    fbdevHWSave(pScrn);
     fbdevHWSaveScreen(pScreen, SCREEN_SAVER_ON);
-    fbdevHWAdjustFrame(scrnIndex,0,0,0);
 
     /* mi layer */
     miClearVisualTypes();
@@ -512,52 +523,50 @@ GlamoScreenInit(int scrnIndex, ScreenPtr pScreen, int argc, char **argv)
         xf86DrvMsg(pScrn->scrnIndex, X_WARNING,
                    "Render extension initialisation failed\n");
 
-        /* map in the registers */
-        pGlamo->reg_base = xf86MapVidMem(pScreen->myNum, VIDMEM_MMIO, 0x8000000, 0x2400);
+    /* map in the registers */
+    pGlamo->reg_base = xf86MapVidMem(pScreen->myNum, VIDMEM_MMIO, 0x8000000, 0x2400);
 
-        pGlamo->pScreen = pScreen;
+    pGlamo->pScreen = pScreen;
 
-        xf86LoadSubModule(pScrn, "exa");
-        xf86LoaderReqSymLists(exaSymbols, NULL);
+    xf86LoadSubModule(pScrn, "exa");
+    xf86LoaderReqSymLists(exaSymbols, NULL);
 
-	if (!GLAMODrawExaInit(pScreen, pScrn)) {
-            xf86DrvMsg(pScrn->scrnIndex, X_ERROR,
+	if (!GLAMODrawInit(pScrn)) {
+        xf86DrvMsg(pScrn->scrnIndex, X_ERROR,
                        "EXA hardware acceleration initialization failed\n");
-            return FALSE;
-        }
-
-        GLAMODrawEnable(pGlamo);
-
-        xf86SetBlackWhitePixels(pScreen);
-        miInitializeBackingStore(pScreen);
-        xf86SetBackingStore(pScreen);
-
-        /* software cursor */
-        miDCInitialize(pScreen, xf86GetPointerScreenFuncs());
+    } else {
+        pGlamo->accel = TRUE;
+    }
 
-        /* colormap */
-        if (!miCreateDefColormap(pScreen)) {
-            xf86DrvMsg(scrnIndex, X_ERROR,
-                       "internal error: miCreateDefColormap failed "
-                       "in GlamoScreenInit()\n");
-            return FALSE;
-        }
+    xf86SetBlackWhitePixels(pScreen);
+    miInitializeBackingStore(pScreen);
+    xf86SetBackingStore(pScreen);
 
-        flags = CMAP_PALETTED_TRUECOLOR;
-        if (!xf86HandleColormaps(pScreen, 256, 8, fbdevHWLoadPaletteWeak(),
-                                 NULL, flags))
-            return FALSE;
+    /* software cursor */
+    miDCInitialize(pScreen, xf86GetPointerScreenFuncs());
 
+    GlamoEnterVT(scrnIndex, 0);
 
     xf86CrtcScreenInit(pScreen);
     xf86RandR12SetRotations(pScreen, RR_Rotate_0 | RR_Rotate_90 |
                                      RR_Rotate_180 | RR_Rotate_270);
+    /* colormap */
+    if (!miCreateDefColormap(pScreen)) {
+        xf86DrvMsg(scrnIndex, X_ERROR,
+                   "internal error: miCreateDefColormap failed "
+                   "in GlamoScreenInit()\n");
+        return FALSE;
+    }
+
+    flags = CMAP_PALETTED_TRUECOLOR;
+    if (!xf86HandleColormaps(pScreen, 256, 8, fbdevHWLoadPaletteWeak(),
+                             NULL, flags))
+        return FALSE;
 
     xf86DPMSInit(pScreen, xf86DPMSSet, 0);
 
     pScreen->SaveScreen = xf86SaveScreen;
 
-    xf86SetDesiredModes(pScrn);
     /* Wrap the current CloseScreen function */
     pGlamo->CloseScreen = pScreen->CloseScreen;
     pScreen->CloseScreen = GlamoCloseScreen;
@@ -573,8 +582,14 @@ GlamoCloseScreen(int scrnIndex, ScreenPtr pScreen)
     ScrnInfoPtr pScrn = xf86Screens[scrnIndex];
     GlamoPtr pGlamo = GlamoPTR(pScrn);
 
-    fbdevHWRestore(pScrn);
+    if (pGlamo->accel)
+        GLAMODrawFini(pScrn);
+
+    if (pScrn->vtSema)
+        GlamoRestoreHW(pScrn);
+
     fbdevHWUnmapVidmem(pScrn);
+
     pScrn->vtSema = FALSE;
 
     pScreen->CreateScreenResources = pGlamo->CreateScreenResources;
@@ -642,6 +657,7 @@ GlamoInitFramebufferDevice(ScrnInfoPtr pScrn, const char *fb_device) {
         goto fail1;
     }
     return TRUE;
+
 fail1:
     close(pGlamo->fb_fd);
     pGlamo->fb_fd = -1;
@@ -649,3 +665,68 @@ fail2:
     return FALSE;
 }
 
+/* Save framebuffer setup and all the glamo registers we are going to touch */
+static void
+GlamoSaveHW(ScrnInfoPtr pScrn) {
+    GlamoPtr pGlamo = GlamoPTR(pScrn);
+    volatile char *mmio = pGlamo->reg_base;
+
+    pGlamo->saved_clock_2d = MMIO_IN16(mmio, GLAMO_REG_CLOCK_2D);
+    pGlamo->saved_clock_isp = MMIO_IN16(mmio, GLAMO_REG_CLOCK_ISP);
+    pGlamo->saved_clock_gen5_1 = MMIO_IN16(mmio, GLAMO_REG_CLOCK_GEN5_1);
+    pGlamo->saved_clock_gen5_2 = MMIO_IN16(mmio, GLAMO_REG_CLOCK_GEN5_2);
+    pGlamo->saved_hostbus_2 = MMIO_IN16(mmio, GLAMO_REG_HOSTBUS(2));
+
+    if (ioctl(pGlamo->fb_fd, FBIOGET_VSCREENINFO, (void*)(&pGlamo->fb_saved_var)) == -1) {
+        xf86DrvMsg(pScrn->scrnIndex, X_ERROR,
+                   "Framebuffer ioctl FBIOGET_FSCREENINFO failed: %s",
+                   strerror(errno));
+    }
+
+}
+
+static void
+GlamoRestoreHW(ScrnInfoPtr pScrn) {
+    GlamoPtr pGlamo = GlamoPTR(pScrn);
+    volatile char *mmio = pGlamo->reg_base;
+
+    if (ioctl(pGlamo->fb_fd, FBIOPUT_VSCREENINFO, (void*)(&pGlamo->fb_saved_var)) == -1) {
+        xf86DrvMsg(pScrn->scrnIndex, X_ERROR,
+                   "Framebuffer ioctl FBIOSET_FSCREENINFO failed: %s",
+                   strerror(errno));
+    }
+
+    MMIO_OUT16(mmio, GLAMO_REG_CLOCK_2D, pGlamo->saved_clock_2d);
+    MMIO_OUT16(mmio, GLAMO_REG_CLOCK_ISP, pGlamo->saved_clock_isp);
+    MMIO_OUT16(mmio, GLAMO_REG_CLOCK_GEN5_1, pGlamo->saved_clock_gen5_1);
+    MMIO_OUT16(mmio, GLAMO_REG_CLOCK_GEN5_2, pGlamo->saved_clock_gen5_2);
+    MMIO_OUT16(mmio, GLAMO_REG_HOSTBUS(2), pGlamo->saved_hostbus_2);
+}
+
+static Bool
+GlamoEnterVT(int scrnIndex, int flags) {
+    ScrnInfoPtr pScrn = xf86Screens[scrnIndex];
+    GlamoPtr pGlamo = GlamoPTR(pScrn);
+
+    GlamoSaveHW(pScrn);
+
+    if (pGlamo->accel)
+        GLAMODrawEnable(pScrn);
+
+    if (!xf86SetDesiredModes(pScrn))
+        return FALSE;
+
+    return TRUE;
+}
+
+
+static void
+GlamoLeaveVT(int scrnIndex, int flags) {
+    ScrnInfoPtr pScrn = xf86Screens[scrnIndex];
+    GlamoPtr pGlamo = GlamoPTR(pScrn);
+
+    if (pGlamo->accel)
+        GLAMODrawDisable(pScrn);
+
+    GlamoRestoreHW(pScrn);
+}
diff --git a/src/glamo-funcs.c b/src/glamo-funcs.c
index 423a2dc..0ef64b8 100644
--- a/src/glamo-funcs.c
+++ b/src/glamo-funcs.c
@@ -22,15 +22,15 @@
  * Author:
  *  Dodji Seketeli <dodji@openedhand.com>
  */
-
 #include <unistd.h>
+#if 0
+
 
 #include "glamo-log.h"
 #include "glamo.h"
 #include "glamo-funcs.h"
 #include "glamo-regs.h"
 #include "glamo-cmdq.h"
-
 void
 GLAMOSetBitMask(GlamoPtr pGlamo, int reg, int mask, int val)
 {
@@ -555,4 +555,4 @@ GLAMOISPDisplayYUVPlanarFrame (GlamoPtr pGlamo,
 }
 
 #endif /*XV*/
-
+#endif
diff --git a/src/glamo.h b/src/glamo.h
index f14224a..84cba3d 100644
--- a/src/glamo.h
+++ b/src/glamo.h
@@ -78,7 +78,7 @@ typedef volatile CARD16        VOL16;
 typedef struct _MemBuf {
 	int size;
 	int used;
-	void *address;
+	char data[1];
 } MemBuf;
 
 typedef struct {
@@ -111,11 +111,22 @@ typedef struct {
 
     /* linux framebuffer */
     int fb_fd;
+    struct fb_var_screeninfo fb_saved_var;
     struct fb_var_screeninfo fb_var;
     struct fb_fix_screeninfo fb_fix;
     unsigned char *fbstart;
 	unsigned char *fbmem;
 	int fboff;
+
+    /* save hardware registers */
+    short saved_clock_2d;
+    short saved_clock_isp;
+    short saved_clock_gen5_1;
+    short saved_clock_gen5_2;
+    short saved_hostbus_2;
+
+    /* Use hardware acceleration */
+    Bool accel;
 } GlamoRec, *GlamoPtr;
 
 #define GlamoPTR(p) ((GlamoPtr)((p)->driverPrivate))
@@ -136,22 +147,16 @@ MMIOSetBitMask(volatile char *mmio, CARD32 reg, CARD16 mask, CARD16 val)
 
 /* glamo_draw.c */
 Bool
-GLAMODrawInit(ScreenPtr pScreen);
-
-void
-GLAMODrawSetup(GlamoPtr pGlamo);
+GLAMODrawInit(ScrnInfoPtr pScrn);
 
-void
-GLAMODrawEnable(GlamoPtr pScreen);
+Bool
+GLAMODrawEnable(ScrnInfoPtr pScrn);
 
 void
-GLAMODrawDisable(ScreenPtr pScreen);
+GLAMODrawDisable(ScrnInfoPtr pScrn);
 
 void
-GLAMODrawFini(ScreenPtr pScreen);
-
-Bool
-GLAMODrawExaInit(ScreenPtr pScreen, ScrnInfoPtr pScrn);
+GLAMODrawFini(ScrnInfoPtr pScrn);
 
 /* glamo-display.h */
 Bool