Kernel handling of command queue

This adds handling of Glamo's ring buffer at the kernel level, to be accessed
via ioctl DRM_GLAMO_CMDBUF.

Signed-off-by: Thomas White <taw@bitwiz.org.uk>

10 files changed, 407 insertions, 43 deletions

diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index 30546a72d75..4bc5cab825b 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -302,13 +302,14 @@ EXPORT_SYMBOL(drm_init);
  *
  */
 
-int drm_platform_init(struct drm_driver *driver, struct platform_device *pdev)
+int drm_platform_init(struct drm_driver *driver, struct platform_device *pdev,
+		      void *priv)
 {
 	DRM_DEBUG("\n");
 
 	INIT_LIST_HEAD(&driver->device_list);
 
-	return drm_get_platform_dev(pdev, driver);
+	return drm_get_platform_dev(pdev, driver, priv);
 }
 
 EXPORT_SYMBOL(drm_platform_init);
diff --git a/drivers/gpu/drm/drm_stub.c b/drivers/gpu/drm/drm_stub.c
index e8aa423bd9b..5f392b7be23 100644
--- a/drivers/gpu/drm/drm_stub.c
+++ b/drivers/gpu/drm/drm_stub.c
@@ -434,7 +434,7 @@ err_g1:
  * \sa drm_get_dev
  */
 int drm_get_platform_dev(struct platform_device *pdev,
-                         struct drm_driver *driver)
+                         struct drm_driver *driver, void *priv)
 {
 	struct drm_device *dev;
 	int ret;
@@ -446,6 +446,7 @@ int drm_get_platform_dev(struct platform_device *pdev,
 	dev = drm_calloc(1, sizeof(*dev), DRM_MEM_STUB);
 	if (!dev)
 		return -ENOMEM;
+	dev->dev_private = priv;
 
 	if ((ret = drm_fill_in_dev(dev, NULL, NULL, driver))) {
 		printk(KERN_ERR "DRM: Fill_in_dev failed.\n");
diff --git a/drivers/mfd/glamo/Makefile b/drivers/mfd/glamo/Makefile
index 0e951ccf200..38066024567 100644
--- a/drivers/mfd/glamo/Makefile
+++ b/drivers/mfd/glamo/Makefile
@@ -1,7 +1,9 @@
 #
-# Makefile for the Smedia Glamo framebuffer driver
+# Makefile for the Smedia Glamo driver(s)
 #
 
+glamo-drm-y	:=  glamo-cmdq.o
+
 obj-$(CONFIG_MFD_GLAMO)			+= glamo-core.o glamo-gpio.o
 obj-$(CONFIG_MFD_GLAMO_SPI)		+= glamo-spi.o
 obj-$(CONFIG_MFD_GLAMO_SPI_GPIO) 	+= glamo-spi-gpio.o
@@ -10,4 +12,3 @@ obj-$(CONFIG_MFD_GLAMO_FB)		+= glamo-fb.o
 obj-$(CONFIG_MFD_GLAMO_SPI_FB)		+= glamo-lcm-spi.o
 obj-$(CONFIG_MFD_GLAMO_MCI)		+= glamo-mci.o
 obj-$(CONFIG_MFD_GLAMO_DRM)		+= glamo-drm.o
-
diff --git a/drivers/mfd/glamo/glamo-cmdq.c b/drivers/mfd/glamo/glamo-cmdq.c
new file mode 100644
index 00000000000..ac2b752fe64
--- /dev/null
+++ b/drivers/mfd/glamo/glamo-cmdq.c
@@ -0,0 +1,230 @@
+/* Smedia Glamo 336x/337x command queue handling
+ *
+ * Copyright (C) 2008-2009 Thomas White <taw@bitwiz.org.uk>
+ * Based on xf86-video-glamo
+ * Copyright  2007 OpenMoko, Inc.
+ * Copyright © 2009 Lars-Peter Clausen <lars@metafoo.de>
+ *
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <drm/drmP.h>
+#include <drm/glamo_drm.h>
+
+#include "glamo-core.h"
+#include "glamo-drm-private.h"
+#include "glamo-regs.h"
+
+
+#define CQ_LEN (GLAMO_CMDQ_SIZE)
+#define CQ_MASK ((CQ_LEN + 1) * 1024 - 1)
+#define CQ_MASKL (CQ_MASK & 0xffff)
+#define CQ_MASKH (CQ_MASK >> 16)
+
+
+static void reg_write(struct glamodrm_handle *gdrm,
+		      u_int16_t reg, u_int16_t val)
+{
+	iowrite16(val, gdrm->reg_base + reg);
+}
+
+
+static u16 reg_read(struct glamodrm_handle *gdrm, u_int16_t reg)
+{
+	return ioread16(gdrm->reg_base + reg);
+}
+
+
+static void
+glamo_cmdq_wait(struct glamodrm_handle *gdrm, enum glamo_engine engine)
+{
+	u16 mask, val, status;
+
+	switch (engine)
+	{
+		case GLAMO_ENGINE_CMDQ:
+			mask = 0x3;
+			val  = mask;
+			break;
+		case GLAMO_ENGINE_ISP:
+			mask = 0x3 | (1 << 8);
+			val  = 0x3;
+			break;
+		case GLAMO_ENGINE_2D:
+			mask = 0x3 | (1 << 4);
+			val  = 0x3;
+			break;
+		case GLAMO_ENGINE_3D:
+			mask = 0x3 | (1 << 5);
+			val  = 0x3;
+			break;
+		case GLAMO_ENGINE_ALL:
+		default:
+			mask = 1 << 2;
+			val  = mask;
+			break;
+	}
+
+	printk(KERN_INFO "Waiting for engine idle...\n");
+	do {
+		status = ioread16(gdrm->reg_base + GLAMO_REG_CMDQ_STATUS);
+	} while ((status & mask) != val);
+	printk(KERN_INFO "Engine(s) idle.\n");
+}
+
+
+/* This is DRM_IOCTL_GLAMO_CMDBUF */
+int glamo_ioctl_cmdbuf(struct drm_device *dev, void *data,
+		       struct drm_file *file_priv)
+{
+	size_t ring_write, ring_read;
+	size_t new_ring_write;
+	struct glamodrm_handle *gdrm;
+	size_t count;
+	drm_glamo_cmd_buffer_t *cbuf = data;
+	u16 *addr;
+
+	printk(KERN_INFO "glamo_ioctl_cmdbuf\n");
+	gdrm = dev->dev_private;
+
+	count = cbuf->bufsz;
+	addr = (u16 *)cbuf->buf;
+
+	/* TODO: Sanitise buffer before doing anything else */
+
+	ring_write = reg_read(gdrm, GLAMO_REG_CMDQ_WRITE_ADDRL);
+	ring_write |= (reg_read(gdrm, GLAMO_REG_CMDQ_WRITE_ADDRH) << 16);
+
+	/* Calculate where we'll end up */
+	new_ring_write = (((ring_write + count) & CQ_MASK) + 1) & ~1;
+
+	/* Wait until there is enough space to queue the cmd buffer */
+	if (new_ring_write > ring_write) {
+		do {
+			ring_read = reg_read(gdrm, GLAMO_REG_CMDQ_READ_ADDRL)
+					& CQ_MASKL;
+			ring_read |= ((reg_read(gdrm, GLAMO_REG_CMDQ_READ_ADDRH)
+					& CQ_MASKH) << 16);
+		} while (ring_read > ring_write && ring_read < new_ring_write);
+    	} else {
+		do {
+	        	ring_read = reg_read(gdrm, GLAMO_REG_CMDQ_READ_ADDRL)
+					& CQ_MASKL;
+			ring_read |= ((reg_read(gdrm, GLAMO_REG_CMDQ_READ_ADDRH)
+					& CQ_MASKH) << 16);
+		} while (ring_read > ring_write || ring_read < new_ring_write);
+	}
+
+	if (ring_write >= new_ring_write) {
+
+		size_t rest_size;
+		int i;
+
+		/* Wrap around */
+		rest_size = GLAMO_CMDQ_SIZE - ring_write; /* Space left */
+
+		/* Write from current position to end */
+		for ( i=0; i<rest_size; i+=2 ) {
+			iowrite16(*(addr+i), gdrm->cmdq_base + ring_write + i);
+		}
+
+		/* Write from start */
+		for ( i=0; i<(count-rest_size); i+=2 ) {
+			iowrite16(*(addr+rest_size+i), gdrm->cmdq_base + i);
+		}
+
+		/* 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) {
+			iowrite16(0x0000, gdrm->cmdq_base);
+			iowrite16(0x0000, gdrm->cmdq_base + 2);
+			new_ring_write = 4;
+		}
+
+		/* Before changing write position, read has to stop */
+		glamo_cmdq_wait(gdrm, GLAMO_ENGINE_CMDQ);
+
+		/* Suppose we just filled the WHOLE ring buffer, and so the
+		 * write position ends up in the same place as it started.
+		 * No change in pointer means no activity from the command
+		 * queue engine.  So, insert a no-op */
+		if (ring_write == new_ring_write) {
+			iowrite16(0x0000, gdrm->cmdq_base + new_ring_write);
+			iowrite16(0x0000, gdrm->cmdq_base + new_ring_write + 2);
+			new_ring_write += 4;
+		}
+
+	} else {
+
+		int i;
+
+		/* The easy case */
+		for ( i=0; i<count; i+=2 ) {
+			iowrite16(*(addr+i), gdrm->cmdq_base + ring_write + i);
+		}
+		glamo_cmdq_wait(gdrm, GLAMO_ENGINE_CMDQ);
+
+	}
+
+	/* Finally, update the write pointer */
+	reg_write(gdrm, GLAMO_REG_CMDQ_WRITE_ADDRH,
+					(new_ring_write >> 16) & CQ_MASKH);
+	reg_write(gdrm, GLAMO_REG_CMDQ_WRITE_ADDRL,
+					new_ring_write & CQ_MASKL);
+
+	glamo_cmdq_wait(gdrm, GLAMO_ENGINE_ALL);
+
+	return 0;
+}
+
+
+int glamo_cmdq_init(struct glamodrm_handle *gdrm)
+{
+	unsigned int i;
+
+	/* Start by zeroing the command queue memory */
+	for ( i=0; i<GLAMO_CMDQ_SIZE; i+=2 ) {
+		iowrite16(0x0000, gdrm->cmdq_base+i);
+	}
+
+	glamo_engine_reset(gdrm->glamo_core, GLAMO_ENGINE_CMDQ);
+
+	/* Set up command queue location */
+	reg_write(gdrm, GLAMO_REG_CMDQ_BASE_ADDRL,
+					GLAMO_OFFSET_CMDQ & 0xffff);
+	reg_write(gdrm, GLAMO_REG_CMDQ_BASE_ADDRH,
+					(GLAMO_OFFSET_CMDQ >> 16) & 0x7f);
+
+	/* Length of command queue in 1k blocks, minus one */
+	reg_write(gdrm, GLAMO_REG_CMDQ_LEN, (GLAMO_CMDQ_SIZE >> 10)-1);
+
+	reg_write(gdrm, GLAMO_REG_CMDQ_WRITE_ADDRH, 0);
+	reg_write(gdrm, GLAMO_REG_CMDQ_WRITE_ADDRL, 0);
+	reg_write(gdrm, GLAMO_REG_CMDQ_READ_ADDRH, 0);
+	reg_write(gdrm, GLAMO_REG_CMDQ_READ_ADDRL, 0);
+	reg_write(gdrm, GLAMO_REG_CMDQ_CONTROL,
+					 1 << 12 |	/* Turbo flip (?) */
+					 5 << 8 |	/* No interrupt */
+					 8 << 4);	/* HQ threshold */
+
+	/* Wait for things to settle down */
+	glamo_cmdq_wait(gdrm, GLAMO_ENGINE_ALL);
+
+	return 0;
+}
diff --git a/drivers/mfd/glamo/glamo-cmdq.h b/drivers/mfd/glamo/glamo-cmdq.h
new file mode 100644
index 00000000000..e57cf459085
--- /dev/null
+++ b/drivers/mfd/glamo/glamo-cmdq.h
@@ -0,0 +1,39 @@
+/* Smedia Glamo 336x/337x command queue handling
+ *
+ * Copyright (c) 2008-2009 Thomas White <taw@bitwiz.org.uk>
+ * Based on xf86-video-glamo
+ * Copyright  2007 OpenMoko, Inc.
+ * Copyright © 2009 Lars-Peter Clausen <lars@metafoo.de>
+ *
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#ifndef __GLAMO_CMDQ_H
+#define __GLAMO_CMDQ_H
+
+#include <drm/drmP.h>
+
+#include "glamo-drm-private.h"
+
+extern int glamo_ioctl_cmdbuf(struct drm_device *dev, void *data,
+			      struct drm_file *file_priv);
+
+extern int glamo_cmdq_init(struct glamo_core *glamo);
+
+
+#endif /* __GLAMO_CMDQ_H */
diff --git a/drivers/mfd/glamo/glamo-core.c b/drivers/mfd/glamo/glamo-core.c
index 5a8fda2404c..aab0825ca31 100644
--- a/drivers/mfd/glamo/glamo-core.c
+++ b/drivers/mfd/glamo/glamo-core.c
@@ -177,6 +177,7 @@ static inline void glamo_vmem_read(struct glamo_core *glamo, u_int16_t *buf,
  ***********************************************************************/
 static struct resource glamo_cmdq_resources[] = {
 	{
+		.name	= "glamo-cmdq-regs",
 		.start  = GLAMO_REGOFS_CMDQUEUE,
 		.end    = GLAMO_REGOFS_RISC - 1,
 		.flags  = IORESOURCE_MEM,
@@ -186,6 +187,12 @@ static struct resource glamo_cmdq_resources[] = {
 		.end    = GLAMO_MEM_BASE + GLAMO_OFFSET_WORK +
 			  GLAMO_WORK_SIZE - 1,
 		.flags  = IORESOURCE_MEM,
+	}, {
+		.name	= "glamo-command-queue",
+		.start  = GLAMO_MEM_BASE + GLAMO_OFFSET_CMDQ,
+		.end    = GLAMO_MEM_BASE + GLAMO_OFFSET_CMDQ +
+			  GLAMO_CMDQ_SIZE - 1,
+		.flags  = IORESOURCE_MEM,
 	},
 };
 
@@ -915,7 +922,7 @@ static void glamo_power(struct glamo_core *glamo,
 {
 	int n;
 	unsigned long flags;
-	
+
 	spin_lock_irqsave(&glamo->lock, flags);
 
 	dev_info(&glamo->pdev->dev, "***** glamo_power -> %d\n", new_state);
@@ -1267,7 +1274,7 @@ static int __init glamo_probe(struct platform_device *pdev)
 	glamo_mmc_dev->name = "glamo-mci";
 	glamo_mmc_dev->dev.parent = &pdev->dev;
 	glamo_mmc_dev->resource = glamo_mmc_resources;
-	glamo_mmc_dev->num_resources = ARRAY_SIZE(glamo_mmc_resources); 
+	glamo_mmc_dev->num_resources = ARRAY_SIZE(glamo_mmc_resources);
 	glamo_mci_def_pdata.pglamo = glamo;
 	mangle_mem_resources(glamo_mmc_dev->resource,
 			     glamo_mmc_dev->num_resources, glamo->mem);
diff --git a/drivers/mfd/glamo/glamo-core.h b/drivers/mfd/glamo/glamo-core.h
index 8ab0491c064..63390492141 100644
--- a/drivers/mfd/glamo/glamo-core.h
+++ b/drivers/mfd/glamo/glamo-core.h
@@ -77,7 +77,8 @@ enum glamo_engine {
 	GLAMO_ENGINE_RISC1,
 	GLAMO_ENGINE_SPI,
 #endif
-	__NUM_GLAMO_ENGINES
+	__NUM_GLAMO_ENGINES,
+	GLAMO_ENGINE_ALL
 };
 
 struct glamo_mci_pdata {
diff --git a/drivers/mfd/glamo/glamo-drm-private.h b/drivers/mfd/glamo/glamo-drm-private.h
new file mode 100644
index 00000000000..f6bc5b17e5e
--- /dev/null
+++ b/drivers/mfd/glamo/glamo-drm-private.h
@@ -0,0 +1,57 @@
+/* Smedia Glamo 336x/337x DRM private bits
+ *
+ * Copyright (C) 2008-2009 Thomas White <taw@bitwiz.org.uk>
+ * Based on xf86-video-glamo
+ * Copyright  2007 OpenMoko, Inc.
+ * Copyright © 2009 Lars-Peter Clausen <lars@metafoo.de>
+ *
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#ifndef __GLAMO_DRMPRIV_H
+#define __GLAMO_DRMPRIV_H
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include "glamo-core.h"
+
+struct glamodrm_handle {
+
+	/* This device */
+	struct device *dev;
+	/* The parent device handle */
+	struct glamo_core *glamo_core;
+
+	/* Command queue registers */
+	struct resource *reg;
+	char __iomem *reg_base;
+
+	/* VRAM region */
+	struct resource *vram;
+	char __iomem *vram_base;
+
+	/* Command queue region */
+	struct resource *cmdq;
+	char __iomem *cmdq_base;
+
+	ssize_t vram_size;
+};
+
+
+#endif /* __GLAMO_DRMPRIV_H */
diff --git a/drivers/mfd/glamo/glamo-drm.c b/drivers/mfd/glamo/glamo-drm.c
index 12e221a70c7..1e910e3433c 100644
--- a/drivers/mfd/glamo/glamo-drm.c
+++ b/drivers/mfd/glamo/glamo-drm.c
@@ -28,20 +28,16 @@
 #include <drm/glamo_drm.h>
 
 #include "glamo-core.h"
+#include "glamo-cmdq.h"
+#include "glamo-drm-private.h"
 
 #define DRIVER_AUTHOR           "Openmoko, Inc."
 #define DRIVER_NAME             "glamo-drm"
 #define DRIVER_DESC             "SMedia Glamo 3362"
-#define DRIVER_DATE             "20090217"
+#define DRIVER_DATE             "20090426"
 
 #define RESSIZE(ressource) (((ressource)->end - (ressource)->start)+1)
 
-static int glamo_ioctl_cmdbuf(struct drm_device *dev, void *data,
-			      struct drm_file *file_priv)
-{
-	printk(KERN_INFO "glamo_ioctl_cmdbuf\n");
-	return 0;
-}
 
 static int glamo_ioctl_swap(struct drm_device *dev, void *data,
 			    struct drm_file *file_priv)
@@ -116,25 +112,8 @@ struct drm_ioctl_desc glamo_ioctls[] = {
 	DRM_IOCTL_DEF(DRM_GLAMO_GEM_UNPIN, glamo_ioctl_gem_unpin, DRM_AUTH),
 	DRM_IOCTL_DEF(DRM_GLAMO_GEM_PREAD, glamo_ioctl_gem_pread, DRM_AUTH),
 	DRM_IOCTL_DEF(DRM_GLAMO_GEM_PWRITE, glamo_ioctl_gem_pwrite, DRM_AUTH),
-	DRM_IOCTL_DEF(DRM_GLAMO_GEM_WAIT_RENDERING, glamo_ioctl_gem_wait_rendering, DRM_AUTH),
-};
-
-struct glamodrm_handle {
-
-	/* This device */
-	struct device *dev;
-	/* The parent device handle */
-	struct glamo_core *glamo_core;
-
-	/* MMIO region */
-	struct resource *reg;
-	char __iomem *base;
-
-	/* VRAM region */
-	struct resource *vram;
-	char __iomem *vram_base;
-
-	ssize_t vram_size;
+	DRM_IOCTL_DEF(DRM_GLAMO_GEM_WAIT_RENDERING,
+				glamo_ioctl_gem_wait_rendering, DRM_AUTH),
 };
 
 static int glamodrm_firstopen(struct drm_device *dev)
@@ -164,14 +143,16 @@ static void glamodrm_lastclose(struct drm_device *dev)
 	DRM_DEBUG("\n");
 }
 
-static int glamodrm_master_create(struct drm_device *dev, struct drm_master *master)
+static int glamodrm_master_create(struct drm_device *dev,
+				  struct drm_master *master)
 {
 	DRM_DEBUG("\n");
 
         return 0;
 }
 
-static void glamodrm_master_destroy(struct drm_device *dev, struct drm_master *master)
+static void glamodrm_master_destroy(struct drm_device *dev,
+				    struct drm_master *master)
 {
 	DRM_DEBUG("\n");
 }
@@ -256,9 +237,10 @@ static int glamodrm_probe(struct platform_device *pdev)
 		rc = -ENOENT;
 		goto out_free;
 	}
-	glamodrm->base = ioremap(glamodrm->reg->start, RESSIZE(glamodrm->reg));
-	if ( !glamodrm->base ) {
-		dev_err(&pdev->dev, "failed to ioremap() MMIO memory\n");
+	glamodrm->reg_base = ioremap(glamodrm->reg->start,
+				     RESSIZE(glamodrm->reg));
+	if ( !glamodrm->reg_base ) {
+		dev_err(&pdev->dev, "failed to ioremap() MMIO registers\n");
 		rc = -ENOENT;
 		goto out_release_regs;
 	}
@@ -280,7 +262,7 @@ static int glamodrm_probe(struct platform_device *pdev)
 	glamodrm->vram_base = ioremap(glamodrm->vram->start,
 						RESSIZE(glamodrm->vram));
 	if ( !glamodrm->vram_base ) {
-		dev_err(&pdev->dev, "failed to ioremap() MMIO memory\n");
+		dev_err(&pdev->dev, "failed to ioremap() VRAM\n");
 		rc = -ENOENT;
 		goto out_release_vram;
 	}
@@ -289,8 +271,30 @@ static int glamodrm_probe(struct platform_device *pdev)
 	printk(KERN_INFO "[glamo-drm] %lli bytes of Glamo RAM to work with\n",
 					(long long int)glamodrm->vram_size);
 
+	/* Find the command queue itself */
+	glamodrm->cmdq = platform_get_resource(pdev, IORESOURCE_MEM, 2);
+	if ( !glamodrm->cmdq ) {
+		dev_err(&pdev->dev, "Unable to find command queue.\n");
+		rc = -ENOENT;
+		goto out_unmap_vram;
+	}
+	glamodrm->cmdq = request_mem_region(glamodrm->cmdq->start,
+					  RESSIZE(glamodrm->cmdq), pdev->name);
+	if ( !glamodrm->cmdq ) {
+		dev_err(&pdev->dev, "failed to request command queue region\n");
+		rc = -ENOENT;
+		goto out_unmap_vram;
+	}
+	glamodrm->cmdq_base = ioremap(glamodrm->cmdq->start,
+						RESSIZE(glamodrm->cmdq));
+	if ( !glamodrm->cmdq_base ) {
+		dev_err(&pdev->dev, "failed to ioremap() command queue\n");
+		rc = -ENOENT;
+		goto out_release_cmdq;
+	}
+
 	/* Initialise DRM */
-	drm_platform_init(&glamodrm_drm_driver, pdev);
+	drm_platform_init(&glamodrm_drm_driver, pdev, (void *)glamodrm_handle);
 
 	/* Enable 2D and 3D */
 	glamo_engine_enable(glamodrm->glamo_core, GLAMO_ENGINE_3D);
@@ -300,12 +304,18 @@ static int glamodrm_probe(struct platform_device *pdev)
 	glamo_engine_reset(glamodrm->glamo_core, GLAMO_ENGINE_2D);
 	msleep(5);
 
+	glamo_cmdq_init(glamodrm->glamo_core);
+
 	return 0;
 
+out_release_cmdq:
+	release_mem_region(glamodrm->cmdq->start, RESSIZE(glamodrm->cmdq));
+out_unmap_vram:
+	iounmap(glamodrm->vram);
 out_release_vram:
 	release_mem_region(glamodrm->vram->start, RESSIZE(glamodrm->vram));
 out_unmap_regs:
-	iounmap(glamodrm->base);
+	iounmap(glamodrm->reg_base);
 out_release_regs:
 	release_mem_region(glamodrm->reg->start, RESSIZE(glamodrm->reg));
 out_free:
@@ -328,13 +338,17 @@ static int glamodrm_remove(struct platform_device *pdev)
 	platform_set_drvdata(pdev, NULL);
 
 	/* Release registers */
-	iounmap(glamodrm->base);
+	iounmap(glamodrm->reg_base);
 	release_mem_region(glamodrm->reg->start, RESSIZE(glamodrm->reg));
 
 	/* Release VRAM */
 	iounmap(glamodrm->vram_base);
 	release_mem_region(glamodrm->vram->start, RESSIZE(glamodrm->vram));
 
+	/* Release command queue  */
+	iounmap(glamodrm->cmdq_base);
+	release_mem_region(glamodrm->cmdq->start, RESSIZE(glamodrm->cmdq));
+
 	kfree(glamodrm);
 
 	return 0;
diff --git a/drivers/mfd/glamo/glamo-regs.h b/drivers/mfd/glamo/glamo-regs.h
index 2328b8ac989..738cb64369b 100644
--- a/drivers/mfd/glamo/glamo-regs.h
+++ b/drivers/mfd/glamo/glamo-regs.h
@@ -629,4 +629,17 @@ enum glamo_core_revisions {
 	GLAMO_CORE_REV_A3		= 0x0003,
 };
 
+enum glamo_register_cq {
+	GLAMO_REG_CMDQ_BASE_ADDRL     = 0x00,
+	GLAMO_REG_CMDQ_BASE_ADDRH     = 0x02,
+	GLAMO_REG_CMDQ_LEN            = 0x04,
+	GLAMO_REG_CMDQ_WRITE_ADDRL    = 0x06,
+	GLAMO_REG_CMDQ_WRITE_ADDRH    = 0x08,
+	GLAMO_REG_CMDQ_FLIP           = 0x0a,
+	GLAMO_REG_CMDQ_CONTROL        = 0x0c,
+	GLAMO_REG_CMDQ_READ_ADDRL     = 0x0e,
+	GLAMO_REG_CMDQ_READ_ADDRH     = 0x10,
+	GLAMO_REG_CMDQ_STATUS         = 0x12,
+};
+
 #endif /* _GLAMO_REGS_H */