openwrt/target/linux/brcm2708/patches-4.19/950-0550-staging-vc-sm-cma-Add-in-allocation-for-VPU-requests.patch

From 275f4673d8c0601e5dbb16e743187d264e7dbed6 Mon Sep 17 00:00:00 2001
From: Dave Stevenson <dave.stevenson@raspberrypi.org>
Date: Fri, 21 Dec 2018 16:50:53 +0000
Subject: [PATCH] staging: vc-sm-cma: Add in allocation for VPU
 requests.

Module has to change from tristate to bool as all CMA functions
are boolean.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.org>
---
 .../staging/vc04_services/vc-sm-cma/Kconfig   |   4 +-
 .../staging/vc04_services/vc-sm-cma/Makefile  |   2 +-
 .../staging/vc04_services/vc-sm-cma/vc_sm.c   | 642 +++++++++++++++---
 .../staging/vc04_services/vc-sm-cma/vc_sm.h   |  30 +-
 .../vc04_services/vc-sm-cma/vc_sm_cma.c       |  99 +++
 .../vc04_services/vc-sm-cma/vc_sm_cma.h       |  39 ++
 .../vc04_services/vc-sm-cma/vc_sm_cma_vchi.c  |  10 +
 .../vc04_services/vc-sm-cma/vc_sm_cma_vchi.h  |   4 +
 .../vc04_services/vc-sm-cma/vc_sm_defs.h      |   2 +
 9 files changed, 723 insertions(+), 109 deletions(-)
 create mode 100644 drivers/staging/vc04_services/vc-sm-cma/vc_sm_cma.c
 create mode 100644 drivers/staging/vc04_services/vc-sm-cma/vc_sm_cma.h

--- a/drivers/staging/vc04_services/vc-sm-cma/Kconfig
+++ b/drivers/staging/vc04_services/vc-sm-cma/Kconfig
@@ -1,6 +1,6 @@
 config BCM_VC_SM_CMA
-	tristate "VideoCore Shared Memory (CMA) driver"
-	depends on BCM2835_VCHIQ
+	bool "VideoCore Shared Memory (CMA) driver"
+	depends on BCM2835_VCHIQ && DMA_CMA
 	select RBTREE
 	select DMA_SHARED_BUFFER
 	help
--- a/drivers/staging/vc04_services/vc-sm-cma/Makefile
+++ b/drivers/staging/vc04_services/vc-sm-cma/Makefile
@@ -3,6 +3,6 @@ ccflags-y += -Idrivers/staging/vc04_serv
 ccflags-y += -D__VCCOREVER__=0
 
 vc-sm-cma-$(CONFIG_BCM_VC_SM_CMA) := \
-	vc_sm.o vc_sm_cma_vchi.o
+	vc_sm.o vc_sm_cma_vchi.o vc_sm_cma.o
 
 obj-$(CONFIG_BCM_VC_SM_CMA) += vc-sm-cma.o
--- a/drivers/staging/vc04_services/vc-sm-cma/vc_sm.c
+++ b/drivers/staging/vc04_services/vc-sm-cma/vc_sm.c
@@ -9,10 +9,21 @@
  * and taking some code for CMA/dmabuf handling from the Android Ion
  * driver (Google/Linaro).
  *
- * This is cut down version to only support import of dma_bufs from
- * other kernel drivers. A more complete implementation of the old
- * vmcs_sm functionality can follow later.
  *
+ * This driver has 3 main uses:
+ * 1) Allocating buffers for the kernel or userspace that can be shared with the
+ *    VPU.
+ * 2) Importing dmabufs from elsewhere for sharing with the VPU.
+ * 3) Allocating buffers for use by the VPU.
+ *
+ * In the first and second cases the native handle is a dmabuf. Releasing the
+ * resource inherently comes from releasing the dmabuf, and this will trigger
+ * unmapping on the VPU. The underlying allocation and our buffer structure are
+ * retained until the VPU has confirmed that it has finished with it.
+ *
+ * For the VPU allocations the VPU is responsible for triggering the release,
+ * and therefore the released message decrements the dma_buf refcount (with the
+ * VPU mapping having already been marked as released).
  */
 
 /* ---- Include Files ----------------------------------------------------- */
@@ -39,6 +50,7 @@
 #include "vc_sm_cma_vchi.h"
 
 #include "vc_sm.h"
+#include "vc_sm_cma.h"
 #include "vc_sm_knl.h"
 
 /* ---- Private Constants and Types --------------------------------------- */
@@ -72,6 +84,7 @@ struct sm_state_t {
 	struct platform_device *pdev;
 
 	struct sm_instance *sm_handle;	/* Handle for videocore service. */
+	struct cma *cma_heap;
 
 	spinlock_t kernelid_map_lock;	/* Spinlock protecting kernelid_map */
 	struct idr kernelid_map;
@@ -80,6 +93,7 @@ struct sm_state_t {
 	struct list_head buffer_list;	/* List of buffer. */
 
 	struct vc_sm_privdata_t *data_knl;  /* Kernel internal data tracking. */
+	struct vc_sm_privdata_t *vpu_allocs; /* All allocations from the VPU */
 	struct dentry *dir_root;	/* Debug fs entries root. */
 	struct sm_pde_t dir_state;	/* Debug fs entries state sub-tree. */
 
@@ -89,6 +103,12 @@ struct sm_state_t {
 	u32 int_trans_id;		/* Interrupted transaction. */
 };
 
+struct vc_sm_dma_buf_attachment {
+	struct device *dev;
+	struct sg_table *table;
+	struct list_head list;
+};
+
 /* ---- Private Variables ----------------------------------------------- */
 
 static struct sm_state_t *sm_state;
@@ -172,12 +192,14 @@ static int vc_sm_cma_global_state_show(s
 				   resource->size);
 			seq_printf(s, "           DMABUF       %p\n",
 				   resource->dma_buf);
-			seq_printf(s, "           ATTACH       %p\n",
-				   resource->attach);
+			if (resource->imported) {
+				seq_printf(s, "           ATTACH       %p\n",
+					   resource->import.attach);
+				seq_printf(s, "           SGT          %p\n",
+					   resource->import.sgt);
+			}
 			seq_printf(s, "           SG_TABLE     %p\n",
 				   resource->sg_table);
-			seq_printf(s, "           SGT          %p\n",
-				   resource->sgt);
 			seq_printf(s, "           DMA_ADDR     %pad\n",
 				   &resource->dma_addr);
 			seq_printf(s, "           VC_HANDLE     %08x\n",
@@ -209,17 +231,33 @@ static void vc_sm_add_resource(struct vc
 }
 
 /*
- * Release an allocation.
- * All refcounting is done via the dma buf object.
+ * Cleans up imported dmabuf.
  */
-static void vc_sm_release_resource(struct vc_sm_buffer *buffer, int force)
+static void vc_sm_clean_up_dmabuf(struct vc_sm_buffer *buffer)
 {
-	mutex_lock(&sm_state->map_lock);
-	mutex_lock(&buffer->lock);
+	if (!buffer->imported)
+		return;
 
-	pr_debug("[%s]: buffer %p (name %s, size %zu)\n",
-		 __func__, buffer, buffer->name, buffer->size);
+	/* Handle cleaning up imported dmabufs */
+	mutex_lock(&buffer->lock);
+	if (buffer->import.sgt) {
+		dma_buf_unmap_attachment(buffer->import.attach,
+					 buffer->import.sgt,
+					 DMA_BIDIRECTIONAL);
+		buffer->import.sgt = NULL;
+	}
+	if (buffer->import.attach) {
+		dma_buf_detach(buffer->dma_buf, buffer->import.attach);
+		buffer->import.attach = NULL;
+	}
+	mutex_unlock(&buffer->lock);
+}
 
+/*
+ * Instructs VPU to decrement the refcount on a buffer.
+ */
+static void vc_sm_vpu_free(struct vc_sm_buffer *buffer)
+{
 	if (buffer->vc_handle && buffer->vpu_state == VPU_MAPPED) {
 		struct vc_sm_free_t free = { buffer->vc_handle, 0 };
 		int status = vc_sm_cma_vchi_free(sm_state->sm_handle, &free,
@@ -230,17 +268,32 @@ static void vc_sm_release_resource(struc
 		}
 
 		if (sm_state->require_released_callback) {
-			/* Need to wait for the VPU to confirm the free */
+			/* Need to wait for the VPU to confirm the free. */
 
 			/* Retain a reference on this until the VPU has
 			 * released it
 			 */
 			buffer->vpu_state = VPU_UNMAPPING;
-			goto defer;
+		} else {
+			buffer->vpu_state = VPU_NOT_MAPPED;
+			buffer->vc_handle = 0;
 		}
-		buffer->vpu_state = VPU_NOT_MAPPED;
-		buffer->vc_handle = 0;
 	}
+}
+
+/*
+ * Release an allocation.
+ * All refcounting is done via the dma buf object.
+ *
+ * Must be called with the mutex held. The function will either release the
+ * mutex (if defering the release) or destroy it. The caller must therefore not
+ * reuse the buffer on return.
+ */
+static void vc_sm_release_resource(struct vc_sm_buffer *buffer)
+{
+	pr_debug("[%s]: buffer %p (name %s, size %zu)\n",
+		 __func__, buffer, buffer->name, buffer->size);
+
 	if (buffer->vc_handle) {
 		/* We've sent the unmap request but not had the response. */
 		pr_err("[%s]: Waiting for VPU unmap response on %p\n",
@@ -248,45 +301,43 @@ static void vc_sm_release_resource(struc
 		goto defer;
 	}
 	if (buffer->in_use) {
-		/* Don't release dmabuf here - we await the release */
+		/* dmabuf still in use - we await the release */
 		pr_err("[%s]: buffer %p is still in use\n",
 		       __func__, buffer);
 		goto defer;
 	}
 
-	/* Handle cleaning up imported dmabufs */
-	if (buffer->sgt) {
-		dma_buf_unmap_attachment(buffer->attach, buffer->sgt,
-					 DMA_BIDIRECTIONAL);
-		buffer->sgt = NULL;
-	}
-	if (buffer->attach) {
-		dma_buf_detach(buffer->dma_buf, buffer->attach);
-		buffer->attach = NULL;
-	}
-
-	/* Release the dma_buf (whether ours or imported) */
-	if (buffer->import_dma_buf) {
-		dma_buf_put(buffer->import_dma_buf);
-		buffer->import_dma_buf = NULL;
-		buffer->dma_buf = NULL;
-	} else if (buffer->dma_buf) {
-		dma_buf_put(buffer->dma_buf);
-		buffer->dma_buf = NULL;
+	/* Release the allocation (whether imported dmabuf or CMA allocation) */
+	if (buffer->imported) {
+		pr_debug("%s: Release imported dmabuf %p\n", __func__,
+			 buffer->import.dma_buf);
+		if (buffer->import.dma_buf)
+			dma_buf_put(buffer->import.dma_buf);
+		else
+			pr_err("%s: Imported dmabuf already been put for buf %p\n",
+			       __func__, buffer);
+		buffer->import.dma_buf = NULL;
+	} else {
+		if (buffer->sg_table) {
+			/* Our own allocation that we need to dma_unmap_sg */
+			dma_unmap_sg(&sm_state->pdev->dev,
+				     buffer->sg_table->sgl,
+				     buffer->sg_table->nents,
+				     DMA_BIDIRECTIONAL);
+		}
+		pr_debug("%s: Release our allocation\n", __func__);
+		vc_sm_cma_buffer_free(&buffer->alloc);
+		pr_debug("%s: Release our allocation - done\n", __func__);
 	}
 
-	if (buffer->sg_table && !buffer->import_dma_buf) {
-		/* Our own allocation that we need to dma_unmap_sg */
-		dma_unmap_sg(&sm_state->pdev->dev, buffer->sg_table->sgl,
-			     buffer->sg_table->nents, DMA_BIDIRECTIONAL);
-	}
 
-	/* Free the local resource. Start by removing it from the list */
-	buffer->private = NULL;
+	/* Free our buffer. Start by removing it from the list */
+	mutex_lock(&sm_state->map_lock);
 	list_del(&buffer->global_buffer_list);
+	mutex_unlock(&sm_state->map_lock);
 
+	pr_debug("%s: Release our allocation - done\n", __func__);
 	mutex_unlock(&buffer->lock);
-	mutex_unlock(&sm_state->map_lock);
 
 	mutex_destroy(&buffer->lock);
 
@@ -295,7 +346,7 @@ static void vc_sm_release_resource(struc
 
 defer:
 	mutex_unlock(&buffer->lock);
-	mutex_unlock(&sm_state->map_lock);
+	return;
 }
 
 /* Create support for private data tracking. */
@@ -317,16 +368,267 @@ static struct vc_sm_privdata_t *vc_sm_cm
 	return file_data;
 }
 
+static struct sg_table *dup_sg_table(struct sg_table *table)
+{
+	struct sg_table *new_table;
+	int ret, i;
+	struct scatterlist *sg, *new_sg;
+
+	new_table = kzalloc(sizeof(*new_table), GFP_KERNEL);
+	if (!new_table)
+		return ERR_PTR(-ENOMEM);
+
+	ret = sg_alloc_table(new_table, table->nents, GFP_KERNEL);
+	if (ret) {
+		kfree(new_table);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	new_sg = new_table->sgl;
+	for_each_sg(table->sgl, sg, table->nents, i) {
+		memcpy(new_sg, sg, sizeof(*sg));
+		sg->dma_address = 0;
+		new_sg = sg_next(new_sg);
+	}
+
+	return new_table;
+}
+
+static void free_duped_table(struct sg_table *table)
+{
+	sg_free_table(table);
+	kfree(table);
+}
+
+/* Dma buf operations for use with our own allocations */
+
+static int vc_sm_dma_buf_attach(struct dma_buf *dmabuf,
+				struct dma_buf_attachment *attachment)
+
+{
+	struct vc_sm_dma_buf_attachment *a;
+	struct sg_table *table;
+	struct vc_sm_buffer *buf = dmabuf->priv;
+
+	a = kzalloc(sizeof(*a), GFP_KERNEL);
+	if (!a)
+		return -ENOMEM;
+
+	table = dup_sg_table(buf->sg_table);
+	if (IS_ERR(table)) {
+		kfree(a);
+		return -ENOMEM;
+	}
+
+	a->table = table;
+	INIT_LIST_HEAD(&a->list);
+
+	attachment->priv = a;
+
+	mutex_lock(&buf->lock);
+	list_add(&a->list, &buf->attachments);
+	mutex_unlock(&buf->lock);
+	pr_debug("%s dmabuf %p attachment %p\n", __func__, dmabuf, attachment);
+
+	return 0;
+}
+
+static void vc_sm_dma_buf_detatch(struct dma_buf *dmabuf,
+				  struct dma_buf_attachment *attachment)
+{
+	struct vc_sm_dma_buf_attachment *a = attachment->priv;
+	struct vc_sm_buffer *buf = dmabuf->priv;
+
+	pr_debug("%s dmabuf %p attachment %p\n", __func__, dmabuf, attachment);
+	free_duped_table(a->table);
+	mutex_lock(&buf->lock);
+	list_del(&a->list);
+	mutex_unlock(&buf->lock);
+
+	kfree(a);
+}
+
+static struct sg_table *vc_sm_map_dma_buf(struct dma_buf_attachment *attachment,
+					  enum dma_data_direction direction)
+{
+	struct vc_sm_dma_buf_attachment *a = attachment->priv;
+	struct sg_table *table;
+
+	table = a->table;
+
+	if (!dma_map_sg(attachment->dev, table->sgl, table->nents,
+			direction))
+		return ERR_PTR(-ENOMEM);
+
+	pr_debug("%s attachment %p\n", __func__, attachment);
+	return table;
+}
+
+static void vc_sm_unmap_dma_buf(struct dma_buf_attachment *attachment,
+				struct sg_table *table,
+				enum dma_data_direction direction)
+{
+	pr_debug("%s attachment %p\n", __func__, attachment);
+	dma_unmap_sg(attachment->dev, table->sgl, table->nents, direction);
+}
+
+static int vc_sm_dmabuf_mmap(struct dma_buf *dmabuf, struct vm_area_struct *vma)
+{
+	struct vc_sm_buffer *buf = dmabuf->priv;
+	struct sg_table *table = buf->sg_table;
+	unsigned long addr = vma->vm_start;
+	unsigned long offset = vma->vm_pgoff * PAGE_SIZE;
+	struct scatterlist *sg;
+	int i;
+	int ret = 0;
+
+	pr_debug("%s dmabuf %p, buf %p, vm_start %08lX\n", __func__, dmabuf,
+		 buf, addr);
+
+	mutex_lock(&buf->lock);
+
+	/* now map it to userspace */
+	for_each_sg(table->sgl, sg, table->nents, i) {
+		struct page *page = sg_page(sg);
+		unsigned long remainder = vma->vm_end - addr;
+		unsigned long len = sg->length;
+
+		if (offset >= sg->length) {
+			offset -= sg->length;
+			continue;
+		} else if (offset) {
+			page += offset / PAGE_SIZE;
+			len = sg->length - offset;
+			offset = 0;
+		}
+		len = min(len, remainder);
+		ret = remap_pfn_range(vma, addr, page_to_pfn(page), len,
+				      vma->vm_page_prot);
+		if (ret)
+			break;
+		addr += len;
+		if (addr >= vma->vm_end)
+			break;
+	}
+	mutex_unlock(&buf->lock);
+
+	if (ret)
+		pr_err("%s: failure mapping buffer to userspace\n",
+		       __func__);
+
+	return ret;
+}
+
+static void vc_sm_dma_buf_release(struct dma_buf *dmabuf)
+{
+	struct vc_sm_buffer *buffer;
+
+	if (!dmabuf)
+		return;
+
+	buffer = (struct vc_sm_buffer *)dmabuf->priv;
+
+	mutex_lock(&buffer->lock);
+
+	pr_debug("%s dmabuf %p, buffer %p\n", __func__, dmabuf, buffer);
+
+	buffer->in_use = 0;
+
+	/* Unmap on the VPU */
+	vc_sm_vpu_free(buffer);
+	pr_debug("%s vpu_free done\n", __func__);
+
+	/* Unmap our dma_buf object (the vc_sm_buffer remains until released
+	 * on the VPU).
+	 */
+	vc_sm_clean_up_dmabuf(buffer);
+	pr_debug("%s clean_up dmabuf done\n", __func__);
+
+	vc_sm_release_resource(buffer);
+	pr_debug("%s done\n", __func__);
+}
+
+static int vc_sm_dma_buf_begin_cpu_access(struct dma_buf *dmabuf,
+					  enum dma_data_direction direction)
+{
+	struct vc_sm_buffer *buf;
+	struct vc_sm_dma_buf_attachment *a;
+
+	if (!dmabuf)
+		return -EFAULT;
+
+	buf = dmabuf->priv;
+	if (!buf)
+		return -EFAULT;
+
+	mutex_lock(&buf->lock);
+
+	list_for_each_entry(a, &buf->attachments, list) {
+		dma_sync_sg_for_cpu(a->dev, a->table->sgl, a->table->nents,
+				    direction);
+	}
+	mutex_unlock(&buf->lock);
+
+	return 0;
+}
+
+static int vc_sm_dma_buf_end_cpu_access(struct dma_buf *dmabuf,
+					enum dma_data_direction direction)
+{
+	struct vc_sm_buffer *buf;
+	struct vc_sm_dma_buf_attachment *a;
+
+	if (!dmabuf)
+		return -EFAULT;
+	buf = dmabuf->priv;
+	if (!buf)
+		return -EFAULT;
+
+	mutex_lock(&buf->lock);
+
+	list_for_each_entry(a, &buf->attachments, list) {
+		dma_sync_sg_for_device(a->dev, a->table->sgl, a->table->nents,
+				       direction);
+	}
+	mutex_unlock(&buf->lock);
+
+	return 0;
+}
+
+static void *vc_sm_dma_buf_kmap(struct dma_buf *dmabuf, unsigned long offset)
+{
+	/* FIXME */
+	return NULL;
+}
+
+static void vc_sm_dma_buf_kunmap(struct dma_buf *dmabuf, unsigned long offset,
+				 void *ptr)
+{
+	/* FIXME */
+}
+
+static const struct dma_buf_ops dma_buf_ops = {
+	.map_dma_buf = vc_sm_map_dma_buf,
+	.unmap_dma_buf = vc_sm_unmap_dma_buf,
+	.mmap = vc_sm_dmabuf_mmap,
+	.release = vc_sm_dma_buf_release,
+	.attach = vc_sm_dma_buf_attach,
+	.detach = vc_sm_dma_buf_detatch,
+	.begin_cpu_access = vc_sm_dma_buf_begin_cpu_access,
+	.end_cpu_access = vc_sm_dma_buf_end_cpu_access,
+	.map = vc_sm_dma_buf_kmap,
+	.unmap = vc_sm_dma_buf_kunmap,
+};
 /* Dma_buf operations for chaining through to an imported dma_buf */
 static
 int vc_sm_import_dma_buf_attach(struct dma_buf *dmabuf,
 				struct dma_buf_attachment *attachment)
 {
-	struct vc_sm_buffer *res = dmabuf->priv;
+	struct vc_sm_buffer *buf = dmabuf->priv;
 
-	if (!res->import_dma_buf)
+	if (!buf->imported)
 		return -EINVAL;
-	return res->import_dma_buf->ops->attach(res->import_dma_buf,
+	return buf->import.dma_buf->ops->attach(buf->import.dma_buf,
 						attachment);
 }
 
@@ -334,22 +636,23 @@ static
 void vc_sm_import_dma_buf_detatch(struct dma_buf *dmabuf,
 				  struct dma_buf_attachment *attachment)
 {
-	struct vc_sm_buffer *res = dmabuf->priv;
+	struct vc_sm_buffer *buf = dmabuf->priv;
 
-	if (!res->import_dma_buf)
+	if (!buf->imported)
 		return;
-	res->import_dma_buf->ops->detach(res->import_dma_buf, attachment);
+	buf->import.dma_buf->ops->detach(buf->import.dma_buf, attachment);
 }
 
 static
 struct sg_table *vc_sm_import_map_dma_buf(struct dma_buf_attachment *attachment,
 					  enum dma_data_direction direction)
 {
-	struct vc_sm_buffer *res = attachment->dmabuf->priv;
+	struct vc_sm_buffer *buf = attachment->dmabuf->priv;
 
-	if (!res->import_dma_buf)
+	if (!buf->imported)
 		return NULL;
-	return res->import_dma_buf->ops->map_dma_buf(attachment, direction);
+	return buf->import.dma_buf->ops->map_dma_buf(attachment,
+						     direction);
 }
 
 static
@@ -357,87 +660,88 @@ void vc_sm_import_unmap_dma_buf(struct d
 				struct sg_table *table,
 				enum dma_data_direction direction)
 {
-	struct vc_sm_buffer *res = attachment->dmabuf->priv;
+	struct vc_sm_buffer *buf = attachment->dmabuf->priv;
 
-	if (!res->import_dma_buf)
+	if (!buf->imported)
 		return;
-	res->import_dma_buf->ops->unmap_dma_buf(attachment, table, direction);
+	buf->import.dma_buf->ops->unmap_dma_buf(attachment, table, direction);
 }
 
 static
 int vc_sm_import_dmabuf_mmap(struct dma_buf *dmabuf, struct vm_area_struct *vma)
 {
-	struct vc_sm_buffer *res = dmabuf->priv;
+	struct vc_sm_buffer *buf = dmabuf->priv;
 
-	pr_debug("%s: mmap dma_buf %p, res %p, imported db %p\n", __func__,
-		 dmabuf, res, res->import_dma_buf);
-	if (!res->import_dma_buf) {
+	pr_debug("%s: mmap dma_buf %p, buf %p, imported db %p\n", __func__,
+		 dmabuf, buf, buf->import.dma_buf);
+	if (!buf->imported) {
 		pr_err("%s: mmap dma_buf %p- not an imported buffer\n",
 		       __func__, dmabuf);
 		return -EINVAL;
 	}
-	return res->import_dma_buf->ops->mmap(res->import_dma_buf, vma);
+	return buf->import.dma_buf->ops->mmap(buf->import.dma_buf, vma);
 }
 
 static
 void vc_sm_import_dma_buf_release(struct dma_buf *dmabuf)
 {
-	struct vc_sm_buffer *res = dmabuf->priv;
+	struct vc_sm_buffer *buf = dmabuf->priv;
 
 	pr_debug("%s: Relasing dma_buf %p\n", __func__, dmabuf);
-	if (!res->import_dma_buf)
+	mutex_lock(&buf->lock);
+	if (!buf->imported)
 		return;
 
-	res->in_use = 0;
+	buf->in_use = 0;
 
-	vc_sm_release_resource(res, 0);
+	vc_sm_vpu_free(buf);
+
+	vc_sm_release_resource(buf);
 }
 
 static
 void *vc_sm_import_dma_buf_kmap(struct dma_buf *dmabuf,
 				unsigned long offset)
 {
-	struct vc_sm_buffer *res = dmabuf->priv;
+	struct vc_sm_buffer *buf = dmabuf->priv;
 
-	if (!res->import_dma_buf)
+	if (!buf->imported)
 		return NULL;
-	return res->import_dma_buf->ops->map(res->import_dma_buf,
-						      offset);
+	return buf->import.dma_buf->ops->map(buf->import.dma_buf, offset);
 }
 
 static
 void vc_sm_import_dma_buf_kunmap(struct dma_buf *dmabuf,
 				 unsigned long offset, void *ptr)
 {
-	struct vc_sm_buffer *res = dmabuf->priv;
+	struct vc_sm_buffer *buf = dmabuf->priv;
 
-	if (!res->import_dma_buf)
+	if (!buf->imported)
 		return;
-	res->import_dma_buf->ops->unmap(res->import_dma_buf,
-					       offset, ptr);
+	buf->import.dma_buf->ops->unmap(buf->import.dma_buf, offset, ptr);
 }
 
 static
 int vc_sm_import_dma_buf_begin_cpu_access(struct dma_buf *dmabuf,
 					  enum dma_data_direction direction)
 {
-	struct vc_sm_buffer *res = dmabuf->priv;
+	struct vc_sm_buffer *buf = dmabuf->priv;
 
-	if (!res->import_dma_buf)
+	if (!buf->imported)
 		return -EINVAL;
-	return res->import_dma_buf->ops->begin_cpu_access(res->import_dma_buf,
-							    direction);
+	return buf->import.dma_buf->ops->begin_cpu_access(buf->import.dma_buf,
+							  direction);
 }
 
 static
 int vc_sm_import_dma_buf_end_cpu_access(struct dma_buf *dmabuf,
 					enum dma_data_direction direction)
 {
-	struct vc_sm_buffer *res = dmabuf->priv;
+	struct vc_sm_buffer *buf = dmabuf->priv;
 
-	if (!res->import_dma_buf)
+	if (!buf->imported)
 		return -EINVAL;
-	return res->import_dma_buf->ops->end_cpu_access(res->import_dma_buf,
+	return buf->import.dma_buf->ops->end_cpu_access(buf->import.dma_buf,
 							  direction);
 }
 
@@ -516,9 +820,8 @@ vc_sm_cma_import_dmabuf_internal(struct
 	memcpy(import.name, VC_SM_RESOURCE_NAME_DEFAULT,
 	       sizeof(VC_SM_RESOURCE_NAME_DEFAULT));
 
-	pr_debug("[%s]: attempt to import \"%s\" data - type %u, addr %pad, size %u\n",
-		 __func__, import.name, import.type, &dma_addr,
-		 import.size);
+	pr_debug("[%s]: attempt to import \"%s\" data - type %u, addr %pad, size %u.\n",
+		 __func__, import.name, import.type, &dma_addr, import.size);
 
 	/* Allocate the videocore buffer. */
 	status = vc_sm_cma_vchi_import(sm_state->sm_handle, &import, &result,
@@ -548,12 +851,14 @@ vc_sm_cma_import_dmabuf_internal(struct
 	buffer->size = import.size;
 	buffer->vpu_state = VPU_MAPPED;
 
-	buffer->import_dma_buf = dma_buf;
+	buffer->imported = 1;
+	buffer->import.dma_buf = dma_buf;
 
-	buffer->attach = attach;
-	buffer->sgt = sgt;
+	buffer->import.attach = attach;
+	buffer->import.sgt = sgt;
 	buffer->dma_addr = dma_addr;
 	buffer->in_use = 1;
+	buffer->kernel_id = import.kernel_id;
 
 	/*
 	 * We're done - we need to export a new dmabuf chaining through most
@@ -594,6 +899,91 @@ error:
 	return ret;
 }
 
+static int vc_sm_cma_vpu_alloc(u32 size, uint32_t align, const char *name,
+			       u32 mem_handle, struct vc_sm_buffer **ret_buffer)
+{
+	DEFINE_DMA_BUF_EXPORT_INFO(exp_info);
+	struct vc_sm_buffer *buffer = NULL;
+	int aligned_size;
+	int ret = 0;
+
+	/* Align to the user requested align */
+	aligned_size = ALIGN(size, align);
+	/* and then to a page boundary */
+	aligned_size = PAGE_ALIGN(aligned_size);
+
+	if (!aligned_size)
+		return -EINVAL;
+
+	/* Allocate local buffer to track this allocation. */
+	buffer = kzalloc(sizeof(*buffer), GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
+
+	mutex_init(&buffer->lock);
+
+	if (vc_sm_cma_buffer_allocate(sm_state->cma_heap, &buffer->alloc,
+				      aligned_size)) {
+		pr_err("[%s]: cma alloc of %d bytes failed\n",
+		       __func__, aligned_size);
+		ret = -ENOMEM;
+		goto error;
+	}
+	buffer->sg_table = buffer->alloc.sg_table;
+
+	pr_debug("[%s]: cma alloc of %d bytes success\n",
+		 __func__, aligned_size);
+
+	if (dma_map_sg(&sm_state->pdev->dev, buffer->sg_table->sgl,
+		       buffer->sg_table->nents, DMA_BIDIRECTIONAL) <= 0) {
+		pr_err("[%s]: dma_map_sg failed\n", __func__);
+		goto error;
+	}
+
+	INIT_LIST_HEAD(&buffer->attachments);
+
+	memcpy(buffer->name, name,
+	       min(sizeof(buffer->name), strlen(name)));
+
+	exp_info.ops = &dma_buf_ops;
+	exp_info.size = aligned_size;
+	exp_info.flags = O_RDWR;
+	exp_info.priv = buffer;
+
+	buffer->dma_buf = dma_buf_export(&exp_info);
+	if (IS_ERR(buffer->dma_buf)) {
+		ret = PTR_ERR(buffer->dma_buf);
+		goto error;
+	}
+	buffer->dma_addr = (uint32_t)sg_dma_address(buffer->sg_table->sgl);
+	if ((buffer->dma_addr & 0xC0000000) != 0xC0000000) {
+		pr_err("%s: Expecting an uncached alias for dma_addr %pad\n",
+		       __func__, &buffer->dma_addr);
+		buffer->dma_addr |= 0xC0000000;
+	}
+	buffer->private = sm_state->vpu_allocs;
+
+	buffer->vc_handle = mem_handle;
+	buffer->vpu_state = VPU_MAPPED;
+	buffer->vpu_allocated = 1;
+	buffer->size = size;
+	/*
+	 * Create an ID that will be passed along with our message so
+	 * that when we service the release reply, we can look up which
+	 * resource is being released.
+	 */
+	buffer->kernel_id = get_kernel_id(buffer);
+
+	vc_sm_add_resource(sm_state->vpu_allocs, buffer);
+
+	*ret_buffer = buffer;
+	return 0;
+error:
+	if (buffer)
+		vc_sm_release_resource(buffer);
+	return ret;
+}
+
 static void
 vc_sm_vpu_event(struct sm_instance *instance, struct vc_sm_result_t *reply,
 		int reply_len)
@@ -612,21 +1002,61 @@ vc_sm_vpu_event(struct sm_instance *inst
 		struct vc_sm_released *release = (struct vc_sm_released *)reply;
 		struct vc_sm_buffer *buffer =
 					lookup_kernel_id(release->kernel_id);
+		if (!buffer) {
+			pr_err("%s: VC released a buffer that is already released, kernel_id %d\n",
+			       __func__, release->kernel_id);
+			break;
+		}
+		mutex_lock(&buffer->lock);
 
-		/*
-		 * FIXME: Need to check buffer is still valid and allocated
-		 * before continuing
-		 */
 		pr_debug("%s: Released addr %08x, size %u, id %08x, mem_handle %08x\n",
 			 __func__, release->addr, release->size,
 			 release->kernel_id, release->vc_handle);
-		mutex_lock(&buffer->lock);
+
 		buffer->vc_handle = 0;
 		buffer->vpu_state = VPU_NOT_MAPPED;
-		mutex_unlock(&buffer->lock);
 		free_kernel_id(release->kernel_id);
 
-		vc_sm_release_resource(buffer, 0);
+		if (buffer->vpu_allocated) {
+			/* VPU allocation, so release the dmabuf which will
+			 * trigger the clean up.
+			 */
+			mutex_unlock(&buffer->lock);
+			dma_buf_put(buffer->dma_buf);
+		} else {
+			vc_sm_release_resource(buffer);
+		}
+	}
+	break;
+	case VC_SM_MSG_TYPE_VC_MEM_REQUEST:
+	{
+		struct vc_sm_buffer *buffer = NULL;
+		struct vc_sm_vc_mem_request *req =
+					(struct vc_sm_vc_mem_request *)reply;
+		struct vc_sm_vc_mem_request_result reply;
+		int ret;
+
+		pr_debug("%s: Request %u bytes of memory, align %d name %s, trans_id %08x\n",
+			 __func__, req->size, req->align, req->name,
+			 req->trans_id);
+		ret = vc_sm_cma_vpu_alloc(req->size, req->align, req->name,
+					  req->vc_handle, &buffer);
+
+		reply.trans_id = req->trans_id;
+		if (!ret) {
+			reply.addr = buffer->dma_addr;
+			reply.kernel_id = buffer->kernel_id;
+			pr_debug("%s: Allocated resource buffer %p, addr %pad\n",
+				 __func__, buffer, &buffer->dma_addr);
+		} else {
+			pr_err("%s: Allocation failed size %u, name %s, vc_handle %u\n",
+			       __func__, req->size, req->name, req->vc_handle);
+			reply.addr = 0;
+			reply.kernel_id = 0;
+		}
+		vc_sm_vchi_client_vc_mem_req_reply(sm_state->sm_handle, &reply,
+						   &sm_state->int_trans_id);
+		break;
 	}
 	break;
 	default:
@@ -645,6 +1075,14 @@ static void vc_sm_connected_init(void)
 
 	pr_info("[%s]: start\n", __func__);
 
+	if (vc_sm_cma_add_heaps(&sm_state->cma_heap) ||
+	    !sm_state->cma_heap) {
+		pr_err("[%s]: failed to initialise CMA heaps\n",
+		       __func__);
+		ret = -EIO;
+		goto err_free_mem;
+	}
+
 	/*
 	 * Initialize and create a VCHI connection for the shared memory service
 	 * running on videocore.
@@ -696,7 +1134,7 @@ static void vc_sm_connected_init(void)
 		goto err_remove_shared_memory;
 	}
 
-	version.version = 1;
+	version.version = 2;
 	ret = vc_sm_cma_vchi_client_version(sm_state->sm_handle, &version,
 					    &version_result,
 					    &sm_state->int_trans_id);
@@ -768,7 +1206,7 @@ static int bcm2835_vc_sm_cma_remove(stru
 int vc_sm_cma_int_handle(void *handle)
 {
 	struct dma_buf *dma_buf = (struct dma_buf *)handle;
-	struct vc_sm_buffer *res;
+	struct vc_sm_buffer *buf;
 
 	/* Validate we can work with this device. */
 	if (!sm_state || !handle) {
@@ -776,8 +1214,8 @@ int vc_sm_cma_int_handle(void *handle)
 		return 0;
 	}
 
-	res = (struct vc_sm_buffer *)dma_buf->priv;
-	return res->vc_handle;
+	buf = (struct vc_sm_buffer *)dma_buf->priv;
+	return buf->vc_handle;
 }
 EXPORT_SYMBOL_GPL(vc_sm_cma_int_handle);
 
@@ -804,7 +1242,7 @@ EXPORT_SYMBOL_GPL(vc_sm_cma_free);
 int vc_sm_cma_import_dmabuf(struct dma_buf *src_dmabuf, void **handle)
 {
 	struct dma_buf *new_dma_buf;
-	struct vc_sm_buffer *res;
+	struct vc_sm_buffer *buf;
 	int ret;
 
 	/* Validate we can work with this device. */
@@ -818,7 +1256,7 @@ int vc_sm_cma_import_dmabuf(struct dma_b
 
 	if (!ret) {
 		pr_debug("%s: imported to ptr %p\n", __func__, new_dma_buf);
-		res = (struct vc_sm_buffer *)new_dma_buf->priv;
+		buf = (struct vc_sm_buffer *)new_dma_buf->priv;
 
 		/* Assign valid handle at this time.*/
 		*handle = new_dma_buf;
--- a/drivers/staging/vc04_services/vc-sm-cma/vc_sm.h
+++ b/drivers/staging/vc04_services/vc-sm-cma/vc_sm.h
@@ -21,6 +21,8 @@
 #include <linux/types.h>
 #include <linux/miscdevice.h>
 
+#include "vc_sm_cma.h"
+
 #define VC_SM_MAX_NAME_LEN 32
 
 enum vc_sm_vpu_mapping_state {
@@ -29,31 +31,51 @@ enum vc_sm_vpu_mapping_state {
 	VPU_UNMAPPING
 };
 
+struct vc_sm_imported {
+	struct dma_buf *dma_buf;
+	struct dma_buf_attachment *attach;
+	struct sg_table *sgt;
+};
+
 struct vc_sm_buffer {
 	struct list_head global_buffer_list;	/* Global list of buffers. */
 
+	/* Index in the kernel_id idr so that we can find the
+	 * mmal_msg_context again when servicing the VCHI reply.
+	 */
+	int kernel_id;
+
 	size_t size;
 
 	/* Lock over all the following state for this buffer */
 	struct mutex lock;
-	struct sg_table *sg_table;
 	struct list_head attachments;
 
 	char name[VC_SM_MAX_NAME_LEN];
 
 	int in_use:1;	/* Kernel is still using this resource */
+	int imported:1;	/* Imported dmabuf */
+
+	struct sg_table *sg_table;
 
 	enum vc_sm_vpu_mapping_state vpu_state;
 	u32 vc_handle;	/* VideoCore handle for this buffer */
+	int vpu_allocated;	/*
+				 * The VPU made this allocation. Release the
+				 * local dma_buf when the VPU releases the
+				 * resource.
+				 */
 
 	/* DMABUF related fields */
-	struct dma_buf *import_dma_buf;
 	struct dma_buf *dma_buf;
-	struct dma_buf_attachment *attach;
-	struct sg_table *sgt;
 	dma_addr_t dma_addr;
 
 	struct vc_sm_privdata_t *private;
+
+	union {
+		struct vc_sm_cma_alloc_data alloc;
+		struct vc_sm_imported import;
+	};
 };
 
 #endif
--- /dev/null
+++ b/drivers/staging/vc04_services/vc-sm-cma/vc_sm_cma.c
@@ -0,0 +1,99 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * VideoCore Shared Memory CMA allocator
+ *
+ * Copyright: 2018, Raspberry Pi (Trading) Ltd
+ *
+ * Based on the Android ION allocator
+ * Copyright (C) Linaro 2012
+ * Author: <benjamin.gaignard@linaro.org> for ST-Ericsson.
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/err.h>
+#include <linux/cma.h>
+#include <linux/scatterlist.h>
+
+#include "vc_sm_cma.h"
+
+/* CMA heap operations functions */
+int vc_sm_cma_buffer_allocate(struct cma *cma_heap,
+			      struct vc_sm_cma_alloc_data *buffer,
+			      unsigned long len)
+{
+	/* len should already be page aligned */
+	unsigned long num_pages = len / PAGE_SIZE;
+	struct sg_table *table;
+	struct page *pages;
+	int ret;
+
+	pages = cma_alloc(cma_heap, num_pages, 0, GFP_KERNEL);
+	if (!pages)
+		return -ENOMEM;
+
+	table = kmalloc(sizeof(*table), GFP_KERNEL);
+	if (!table)
+		goto err;
+
+	ret = sg_alloc_table(table, 1, GFP_KERNEL);
+	if (ret)
+		goto free_mem;
+
+	sg_set_page(table->sgl, pages, len, 0);
+
+	buffer->priv_virt = pages;
+	buffer->sg_table = table;
+	buffer->cma_heap = cma_heap;
+	buffer->num_pages = num_pages;
+	return 0;
+
+free_mem:
+	kfree(table);
+err:
+	cma_release(cma_heap, pages, num_pages);
+	return -ENOMEM;
+}
+
+void vc_sm_cma_buffer_free(struct vc_sm_cma_alloc_data *buffer)
+{
+	struct cma *cma_heap = buffer->cma_heap;
+	struct page *pages = buffer->priv_virt;
+
+	/* release memory */
+	if (cma_heap)
+		cma_release(cma_heap, pages, buffer->num_pages);
+
+	/* release sg table */
+	if (buffer->sg_table) {
+		sg_free_table(buffer->sg_table);
+		kfree(buffer->sg_table);
+		buffer->sg_table = NULL;
+	}
+}
+
+int __vc_sm_cma_add_heaps(struct cma *cma, void *priv)
+{
+	struct cma **heap = (struct cma **)priv;
+	const char *name = cma_get_name(cma);
+
+	if (!(*heap)) {
+		phys_addr_t phys_addr = cma_get_base(cma);
+
+		pr_debug("%s: Adding cma heap %s (start %pap, size %lu) for use by vcsm\n",
+			 __func__, name, &phys_addr, cma_get_size(cma));
+		*heap = cma;
+	} else {
+		pr_err("%s: Ignoring heap %s as already set\n",
+		       __func__, name);
+	}
+
+	return 0;
+}
+
+int vc_sm_cma_add_heaps(struct cma **cma_heap)
+{
+	cma_for_each_area(__vc_sm_cma_add_heaps, cma_heap);
+	return 0;
+}
--- /dev/null
+++ b/drivers/staging/vc04_services/vc-sm-cma/vc_sm_cma.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+
+/*
+ * VideoCore Shared Memory CMA allocator
+ *
+ * Copyright: 2018, Raspberry Pi (Trading) Ltd
+ *
+ * Based on the Android ION allocator
+ * Copyright (C) Linaro 2012
+ * Author: <benjamin.gaignard@linaro.org> for ST-Ericsson.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+#ifndef VC_SM_CMA_H
+#define VC_SM_CMA_H
+
+struct vc_sm_cma_alloc_data {
+	struct cma *cma_heap;
+	unsigned long num_pages;
+	void *priv_virt;
+	struct sg_table *sg_table;
+};
+
+int vc_sm_cma_buffer_allocate(struct cma *cma_heap,
+			      struct vc_sm_cma_alloc_data *buffer,
+			      unsigned long len);
+void vc_sm_cma_buffer_free(struct vc_sm_cma_alloc_data *buffer);
+
+int vc_sm_cma_add_heaps(struct cma **cma_heap);
+
+#endif
--- a/drivers/staging/vc04_services/vc-sm-cma/vc_sm_cma_vchi.c
+++ b/drivers/staging/vc04_services/vc-sm-cma/vc_sm_cma_vchi.c
@@ -500,3 +500,13 @@ int vc_sm_cma_vchi_client_version(struct
 				   msg, sizeof(*msg), NULL, 0,
 				   cur_trans_id, 0);
 }
+
+int vc_sm_vchi_client_vc_mem_req_reply(struct sm_instance *handle,
+				       struct vc_sm_vc_mem_request_result *msg,
+				       uint32_t *cur_trans_id)
+{
+	return vc_sm_cma_vchi_send_msg(handle,
+				       VC_SM_MSG_TYPE_VC_MEM_REQUEST_REPLY,
+				       msg, sizeof(*msg), 0, 0, cur_trans_id,
+				       0);
+}
--- a/drivers/staging/vc04_services/vc-sm-cma/vc_sm_cma_vchi.h
+++ b/drivers/staging/vc04_services/vc-sm-cma/vc_sm_cma_vchi.h
@@ -56,4 +56,8 @@ int vc_sm_cma_vchi_client_version(struct
 				  struct vc_sm_result_t *result,
 				  u32 *cur_trans_id);
 
+int vc_sm_vchi_client_vc_mem_req_reply(struct sm_instance *handle,
+				       struct vc_sm_vc_mem_request_result *msg,
+				       uint32_t *cur_trans_id);
+
 #endif /* __VC_SM_CMA_VCHI_H__INCLUDED__ */
--- a/drivers/staging/vc04_services/vc-sm-cma/vc_sm_defs.h
+++ b/drivers/staging/vc04_services/vc-sm-cma/vc_sm_defs.h
@@ -264,6 +264,8 @@ struct vc_sm_vc_mem_request {
 	u32 align;
 	/* resource name (for easier tracking) */
 	char     name[VC_SM_RESOURCE_NAME];
+	/* VPU handle for the resource */
+	u32 vc_handle;
 };
 
 /* Response from the kernel to provide the VPU with some memory */