Update vmeta to BMMv2HEADv2.0master

Update vmeta to use the dma_buf handling now provided by libbmm v2.
This permits more flexible buffer management, as the buffers can now
be passed via a standardized mechanism to other subsystems (such as
DRM), and image data to be encoded can be accepted directly from
other subsystems without needing to be copied.

Signed-off-by: Russell King <rmk@arm.linux.org.uk>

1 files changed, 321 insertions, 33 deletions

diff --git a/vmeta_lib.c b/vmeta_lib.c
index 6959a1f..735e8ce 100644
--- a/vmeta_lib.c
+++ b/vmeta_lib.c
@@ -23,6 +23,7 @@
 #include <sys/mman.h>
 #include <sys/ioctl.h>
 #include <sys/poll.h>
+#include <assert.h>
 #include <errno.h>
 #include <fcntl.h>
 #include <stdarg.h>
@@ -36,6 +37,7 @@
 
 #include "vmeta_lib.h"
 #include "bmm_lib.h"
+#include "rb.h"
 #include "uio_vmeta.h"
 
 #define ALIGN(x,a)		__ALIGN_MASK(x,(typeof(x))(a)-1)
@@ -76,9 +78,68 @@ typedef struct vdec_os_driver_cb_s {
 
 // global variable
 static vdec_os_driver_cb_t *vdec_iface = NULL;
+static int vmeta_fd = -1;
 static UNSG32 globalDbgLevel = VDEC_DEBUG_NONE;
 static UNSG32 syncTimeout = 500;
 static pthread_mutex_t pmt = PTHREAD_MUTEX_INITIALIZER;
+static pthread_mutex_t rb_mutex = PTHREAD_MUTEX_INITIALIZER;
+static Rb_node phys_rb, virt_rb;
+
+typedef unsigned long phys_t;
+
+struct v2p {
+	void *vaddr;
+	size_t size;
+	phys_t paddr;
+	int32_t fd;
+	int32_t id;
+};
+
+static int cmp_phys(const void *key, const void *val)
+{
+	const struct v2p *v2p = val;
+	phys_t k = *(const phys_t *)key;
+
+	return (k < v2p->paddr) ? -1 : (k - v2p->paddr < v2p->size) ? 0 : 1;
+}
+
+static struct v2p *find_phys(phys_t paddr)
+{
+	int found = 0;
+	Rb_node node = rb_find_key_n(phys_rb, &paddr, cmp_phys, &found);
+
+	return found ? rb_val(node) : NULL;
+}
+
+static struct v2p *find_remove_phys(phys_t paddr)
+{
+	struct v2p *v2p = NULL;
+	int found = 0;
+	Rb_node node = rb_find_key_n(phys_rb, &paddr, cmp_phys, &found);
+
+	if (found) {
+		v2p = rb_val(node);
+		rb_delete_node(node);
+	}
+
+	return v2p;
+}
+
+static int cmp_virt(const void *key, const void *val)
+{
+	const struct v2p *v2p = val;
+	void *k = (void *)key;
+
+	return (k < v2p->vaddr) ? -1 : (k - v2p->vaddr < v2p->size) ? 0 : 1;
+}
+
+static struct v2p *find_virt(void *vaddr)
+{
+	int found = 0;
+	Rb_node node = rb_find_key_n(virt_rb, vaddr, cmp_virt, &found);
+
+	return found ? rb_val(node) : NULL;
+}
 
 UNSG8 vdec_os_api_rd8(UNSG32 addr)
 {
@@ -116,15 +177,191 @@ UNSG32 vdec_os_api_get_regbase_addr(void)
 	return (UNSG32) vdec->regs;
 }
 
+static void __attribute__((destructor)) vdec_os_api_exit(void)
+{
+	if (phys_rb) {
+		Rb_node node;
+
+		/*
+		 * Release all resources on the virtual rb tree, but leave
+		 * the v2p structs in place, as these will also be on the
+		 * phys rb tree.
+		 */
+		rb_traverse(node, virt_rb) {
+			struct v2p *v2p = rb_val(node);
+
+			ioctl(vmeta_fd, VMETA_CMD_DMABUF_RELEASE, v2p->id);
+			bmm_dmabuf_unmap(v2p->vaddr);
+			bmm_dmabuf_free(v2p->fd);
+			v2p->id = -1;
+		}
+		rb_free_tree(virt_rb);
+
+		rb_traverse(node, phys_rb) {
+			struct v2p *v2p = rb_val(node);
+
+			if (v2p->id != -1) {
+				ioctl(vmeta_fd, VMETA_CMD_DMABUF_RELEASE, v2p->id);
+				v2p->id = -1;
+			}
+			free(v2p);
+		}
+		rb_free_tree(phys_rb);
+	}
+				
+	if (vmeta_fd >= 0)
+		close(vmeta_fd);
+	bmm_exit();
+}
+
+static int vdec_os_api_init(void)
+{
+	int fd;
+
+	if (!virt_rb) {
+		virt_rb = make_rb();
+		phys_rb = make_rb();
+		if (!virt_rb || !phys_rb)
+			goto rb;
+
+		fd = open("/dev/vmeta", O_RDWR | O_CLOEXEC);
+		if (fd == -1)
+			goto rb;
+
+		if (bmm_init() < 0)
+			goto vmeta;
+
+		vmeta_fd = fd;
+	}
+	return 0;
+
+vmeta:
+	close(fd);
+rb:
+	if (virt_rb)
+		rb_free_tree(virt_rb);
+	if (phys_rb)
+		rb_free_tree(phys_rb);
+	phys_rb = virt_rb = NULL;
+
+	return 1;
+}
+
+static int vdec_os_api_add_mapping(void *ptr, size_t size, phys_t paddr,
+	int fd, int id)
+{
+	struct v2p *v2p;
+	Rb_node node;
+	int found = 0;
+
+	v2p = malloc(sizeof(*v2p));
+	if (!v2p)
+		return -1;
+
+	v2p->vaddr = ptr;
+	v2p->size = size;
+	v2p->paddr = paddr;
+	v2p->fd = fd;
+	v2p->id = id;
+
+	pthread_mutex_lock(&rb_mutex);
+	node = rb_find_key_n(phys_rb, &paddr, cmp_phys, &found);
+	assert(found == 0);
+	rb_insert_b(node, v2p);
+
+	if (ptr) {
+		node = rb_find_key_n(virt_rb, ptr, cmp_virt, &found);
+		assert(found == 0);
+		rb_insert_b(node, v2p);
+	}
+	pthread_mutex_unlock(&rb_mutex);
+
+	return 0;
+}
+
+/**
+ * vdec_os_api_dmabuf_import - import a DMA buffer into vmeta
+ * @fd: dmabuf file descriptor
+ * @phys: pointer to unsigned long for physical address of buffer
+ * @size: pointer ti size_t for size of buffer
+ *
+ * Returns: ID of buffer, physical address in *phys and size in *size,
+ * or -1 on error.
+ */
+int vdec_os_api_dmabuf_import(int fd, unsigned long *phys, size_t *size)
+{
+	struct vmeta_dmabuf_import arg;
+	int ret;
+
+	if (vdec_os_api_init())
+		return -1;
+
+	arg.fd = fd;
+	ret = ioctl(vmeta_fd, VMETA_CMD_DMABUF_IMPORT, &arg);
+	if (ret == -1)
+		return ret;
+
+	if ((phys_t)arg.phys != arg.phys) {
+		ret = -1;
+		goto release;
+	}
+
+	ret = vdec_os_api_add_mapping(NULL, arg.size, arg.phys, arg.fd, arg.id);
+	if (ret)
+		goto release;
+
+	return arg.id;
+
+ release:
+	ioctl(vmeta_fd, VMETA_CMD_DMABUF_RELEASE, arg.id);
+	return ret;
+}
+
+void vdec_os_api_dmabuf_release(unsigned long phys)
+{
+	struct v2p *v2p;
+
+	pthread_mutex_lock(&rb_mutex);
+	v2p = find_remove_phys(phys);
+	pthread_mutex_unlock(&rb_mutex);
+
+	ioctl(vmeta_fd, VMETA_CMD_DMABUF_RELEASE, v2p->id);
+	free(v2p);
+}
+
 //Mem map to bmm_lib
-UNSG32 vdec_os_api_get_pa(UNSG32 vaddr)
+UNSG32 vdec_os_api_get_pa(UNSG32 virt)
 {
-	return bmm_get_paddr((void *)vaddr);
+	unsigned long paddr;
+	struct v2p *v2p;
+	void *vaddr = (void *)virt;
+
+	pthread_mutex_lock(&rb_mutex);
+	v2p = find_virt(vaddr);
+	pthread_mutex_unlock(&rb_mutex);
+
+	assert(v2p);
+	paddr = v2p->paddr + vaddr - v2p->vaddr;
+
+	return paddr;
 }
 
 UNSG32 vdec_os_api_get_va(UNSG32 paddr)
-{	
-	return ((UNSG32)bmm_get_vaddr(paddr));
+{
+	struct v2p *v2p;
+	void *vaddr;
+
+	pthread_mutex_lock(&rb_mutex);
+	v2p = find_phys(paddr);
+	pthread_mutex_unlock(&rb_mutex);
+
+	assert(v2p);
+	if (v2p->vaddr)
+		vaddr = v2p->vaddr + paddr - v2p->paddr;
+	else
+		vaddr = NULL;
+
+	return (UNSG32)vaddr;
 }
 
 void vdec_os_api_vfree(void *ptr)
@@ -162,33 +399,83 @@ void *vdec_os_api_vmalloc(UNSG32 size, UNSG32 align)
 
 void vdec_os_api_dma_free(void *ptr)
 {
-	bmm_free(ptr);
+	struct v2p *v2p;
+	Rb_node node;
+	int found = 0;
+
+	dbg_printf(VDEC_DEBUG_MEM, "vdec_os_api_free: 0x%p\n", ptr);
+
+	pthread_mutex_lock(&rb_mutex);
+	node = rb_find_key_n(virt_rb, ptr, cmp_virt, &found);
+	if (!found) {
+		pthread_mutex_unlock(&rb_mutex);
+		dbg_printf(VDEC_DEBUG_MEM, "vdec_os_api_free: not found\n");
+		assert(0);
+		return;
+	}
+	rb_delete_node(node);
+
+	v2p = rb_val(node);
+	assert(find_remove_phys(v2p->paddr) == v2p);
+	pthread_mutex_unlock(&rb_mutex);
+
+	bmm_dmabuf_unmap(v2p->vaddr);
+	if (v2p->id != -1)
+		ioctl(vmeta_fd, VMETA_CMD_DMABUF_RELEASE, v2p->id);
+	bmm_dmabuf_free(v2p->fd);
+	free(v2p);
 }
 
 static void *vmeta_bmm_malloc_aligned(UNSG32 size, UNSG32 align, UNSG32 *phys, int attr)
 {
+	struct vmeta_dmabuf_import arg;
 	unsigned long paddr;
 	void *ptr;
+	int fd;
+
+	dbg_printf(VDEC_DEBUG_MEM, "%s: size 0x%x attr %u align %u\n",
+		   __FUNCTION__, size, attr, align);
 
 	if (size == 0)
 		return NULL;
 
-	dbg_printf(VDEC_DEBUG_MEM, "%s: size 0x%x attr %u align %u\n",
-		   __FUNCTION__, size, attr, align);
+	if (vdec_os_api_init())
+		return NULL;
 
-	ptr = bmm_malloc_aligned_phys(size, attr, align, &paddr);
-	if (!ptr) {
-		dbg_printf(VDEC_DEBUG_MEM, "%s: not enough memory\n",
-			   __FUNCTION__);
+	fd = bmm_dmabuf_alloc(size, attr, align);
+	if (fd < 0) {
+		dbg_printf(VDEC_DEBUG_MEM, "%s: %s\n",
+			   __FUNCTION__, strerror(errno));
 		return NULL;
-	} 
+	}
 
-	*phys = paddr;
+	ptr = bmm_dmabuf_map(fd, 0, size);
+	if (ptr == NULL)
+		goto free;
+
+	arg.fd = fd;
+	if (ioctl(vmeta_fd, VMETA_CMD_DMABUF_IMPORT, &arg) == -1)
+		goto unmap;
+
+	paddr = arg.phys;
+
+	dbg_printf(VDEC_DEBUG_MEM, "%s: virt=%p phys=0x%08lx fd %d id %d\n",
+		   __FUNCTION__, ptr, paddr, fd, arg.id);
+
+	if (vdec_os_api_add_mapping(ptr, size, paddr, fd, arg.id))
+		goto release;
 
-	dbg_printf(VDEC_DEBUG_MEM, "%s: virt=%p phys=0x%08lx\n",
-		   __FUNCTION__, ptr, paddr);
+	*phys = paddr;
 
 	return ptr;
+
+release:
+	ioctl(vmeta_fd, VMETA_CMD_DMABUF_RELEASE, arg.id);
+unmap:
+	bmm_dmabuf_unmap(ptr);
+free:
+	bmm_dmabuf_free(fd);
+	return NULL;
 }
 
 void *vdec_os_api_dma_alloc(UNSG32 size, UNSG32 align, UNSG32 *phys)
@@ -208,11 +495,11 @@ void *vdec_os_api_dma_alloc_writecombine(UNSG32 size, UNSG32 align, UNSG32 *phys
 
 UNSG32 vdec_os_api_flush_cache(UNSG32 vaddr, UNSG32 size, enum dma_data_direction direction)
 {
-	int bmm_direction;
+	struct v2p *v2p;
+	unsigned bmm_direction, offset;
 
 	switch (direction) {
 	case DMA_NONE:
-	default:
 		bmm_direction = BMM_DMA_NONE;
 		break;
 	case DMA_FROM_DEVICE:
@@ -224,16 +511,22 @@ UNSG32 vdec_os_api_flush_cache(UNSG32 vaddr, UNSG32 size, enum dma_data_directio
 	case DMA_BIDIRECTIONAL:
 		bmm_direction = BMM_DMA_BIDIRECTIONAL;
 		break;
+	default:
+		assert(0);
 	}
 
 	if (bmm_direction == BMM_DMA_NONE)
 		return 0;
 
-	if (0 < size) {
-		bmm_flush_cache_range((void *)vaddr, size, bmm_direction);
-	} else {
-		bmm_flush_cache((void *)vaddr, bmm_direction);
-	}
+	pthread_mutex_lock(&rb_mutex);
+	v2p = find_virt((void *)vaddr);
+	pthread_mutex_unlock(&rb_mutex);
+
+	assert(v2p);
+	offset = (void *)vaddr - v2p->vaddr;
+
+	bmm_dmabuf_flush(v2p->fd, v2p->vaddr, offset, size - offset, direction);
+
 	return 0;
 }
 
@@ -346,6 +639,11 @@ SIGN32 vdec_os_driver_init(void)
 		return VDEC_OS_DRIVER_OK;
 	}
 
+	if (vdec_os_api_init()) {
+		pthread_mutex_unlock(&pmt);
+		return -VDEC_OS_DRIVER_INIT_FAIL;
+	}
+
 #if (VMETA_LOG_ON && 0)
 	fp_log = fopen(VMETA_LOG_FILE,"w");
 	if(fp_log == NULL) {
@@ -366,12 +664,7 @@ SIGN32 vdec_os_driver_init(void)
 	// initialize reference count
 	vdec->refcount++;
 
-	// Try to open the (new) vmeta device
-	vdec->fd = open("/dev/vmeta", O_RDWR | O_CLOEXEC);
-	if (vdec->fd == -1) {
-		ret = -VDEC_OS_DRIVER_OPEN_FAIL;
-		goto err_open_fail;
-	}
+	vdec->fd = vmeta_fd;
 
 	ret = ioctl(vdec->fd, VMETA_CMD_GET_INFO, &info);
 	if (ret == -1) {
@@ -412,7 +705,6 @@ SIGN32 vdec_os_driver_init(void)
 	return VDEC_OS_DRIVER_OK;
 
 err_mmap_fail:
-	close(vdec->fd);
 err_open_fail:
 	free(vdec);
 
@@ -461,10 +753,6 @@ SIGN32 vdec_os_driver_clean(void)
 		munmap(vdec->vdec_obj_va, vdec->vdec_obj_size);
 	}
 
-	// close fd
-	close(vdec->fd);
-	dbg_printf(VDEC_DEBUG_ALL, "kernel close\n");
-
 	// free vdec_iface
 	free(vdec);
 	dbg_printf(VDEC_DEBUG_ALL, "free vdec_iface\n");