Better tracking of phys/virt addresses

Implement better tracking of phys/virt address usage.  This permits us
to track multiple virtual mappings for the same physical allocation.

1 files changed, 240 insertions, 88 deletions

diff --git a/bmm_lib.c b/bmm_lib.c
index 93c6182..86f16cd 100644
--- a/bmm_lib.c
+++ b/bmm_lib.c
@@ -40,6 +40,7 @@
 #define API_COMP(x)		((x) & 0xffff)
 #define API_FEAT_GET_DMABUF_FD	0x0001
 #define API_FEAT_FREE_PHYS	0x0002
+#define API_FEAT_KEEP_FORK	0x0003
 
 static unsigned bmm_api;
 static int bmm_fd = -1;
@@ -47,113 +48,141 @@ static pthread_mutex_t bmm_mutex = PTHREAD_MUTEX_INITIALIZER;
 static Rb_node virt_rb;
 static Rb_node phys_rb;
 
-struct bmm_buffer {
-	void *vaddr;
+struct bmm_virt_buffer;
+
+struct bmm_phys_buffer {
 	unsigned long paddr;
 	size_t size;
 	int attr;
+	struct bmm_virt_buffer *virt;
+	unsigned attach;
+};
+
+struct bmm_virt_buffer {
+	void *vaddr;
+	size_t size;
+	struct bmm_phys_buffer *phys;
 };
 
 static int cmp_virt(const void *key, const void *val)
 {
-	const struct bmm_buffer *buf = val;
+	const struct bmm_virt_buffer *vbuf = val;
 	void *k = (void *)key;
 
-	return (k < buf->vaddr) ? -1 :
-	       (k - buf->vaddr < buf->size) ? 0 : 1;
+	return (k < vbuf->vaddr) ? -1 :
+	       (k - vbuf->vaddr < vbuf->size) ? 0 : 1;
 }
 
 static int cmp_phys(const void *key, const void *val)
 {
-	const struct bmm_buffer *buf = val;
+	const struct bmm_phys_buffer *pbuf = val;
 	unsigned long k = (unsigned long)key;
 
-	return (k < buf->paddr) ? -1 :
-	       (k - buf->paddr < buf->size) ? 0 : 1;
+	return (k < pbuf->paddr) ? -1 :
+	       (k - pbuf->paddr < pbuf->size) ? 0 : 1;
 }
 
-static struct bmm_buffer *bmm_buf_find_virt(void *virt)
+static struct bmm_virt_buffer *bmm_buf_find_virt(void *virt)
 {
-	struct bmm_buffer *buf = NULL;
+	struct bmm_virt_buffer *vbuf = NULL;
 	Rb_node node;
 	int found = 0;
 
 	node = rb_find_key_n(virt_rb, virt, cmp_virt, &found);
 	if (found) {
-		buf = rb_val(node);
+		vbuf = rb_val(node);
 
 		pr_debug("rb: %s(%p) phys=0x%08lx virt=%p size=0x%08zx\n",
-			"find_virt", virt, buf->paddr, buf->vaddr, buf->size);
+			 "find_virt", virt, vbuf->phys->paddr, vbuf->vaddr,
+			 vbuf->size);
 	} else {
 		pr_debug("rb: %s(%p): not found\n",
 			"find_virt", virt);
 	}
 
-	return buf;
+	return vbuf;
 }
 
-static struct bmm_buffer *bmm_buf_find_phys(unsigned long phys)
+static struct bmm_phys_buffer *bmm_buf_find_phys(unsigned long phys)
 {
-	struct bmm_buffer *buf = NULL;
+	struct bmm_phys_buffer *pbuf = NULL;
 	Rb_node node;
 	int found = 0;
 
 	node = rb_find_key_n(phys_rb, (void *)phys, cmp_phys, &found);
 	if (found) {
-		buf = rb_val(node);
+		pbuf = rb_val(node);
 
-		pr_debug("rb: %s(0x%08lx) phys=0x%08lx virt=%p size=0x%08zx\n",
-			"find_phys", (unsigned long)phys, buf->paddr, buf->vaddr, buf->size);
+		pr_debug("rb: %s(0x%08lx) phys=0x%08lx size=0x%08zx\n",
+			"find_phys", (unsigned long)phys, pbuf->paddr, pbuf->size);
 	} else {
 		pr_debug("rb: %s(0x%08lx): not found\n",
 			"find_phys", (unsigned long)phys);
 	}
 
-	return buf;
+	return pbuf;
 }
 
-static void bmm_buf_remove(struct bmm_buffer *buf)
+static void bmm_rb_phys_remove(struct bmm_phys_buffer *pbuf)
 {
 	Rb_node node;
 	int found = 0;
 
-	pr_debug("rb: %s phys=0x%08lx virt=%p size=0x%08zx\n",
-		"remove", buf->paddr, buf->vaddr, buf->size);
-
-	node = rb_find_key_n(virt_rb, buf->vaddr, cmp_virt, &found);
-	assert(found);
-	rb_delete_node(node);
+	pr_debug("%s: phys=0x%08lx size=0x%08zx\n",
+		 __FUNCTION__, pbuf->paddr, pbuf->size);
 
-	node = rb_find_key_n(phys_rb, (void *)buf->paddr, cmp_phys, &found);
+	node = rb_find_key_n(phys_rb, (void *)pbuf->paddr, cmp_phys, &found);
 	assert(found);
 	rb_delete_node(node);
 }
 
-static void bmm_buf_insert(struct bmm_buffer *buf)
+static void bmm_rb_phys_insert(struct bmm_phys_buffer *pbuf)
 {
 	Rb_node node;
 	int found = 0;
 
-	pr_debug("rb: %s phys=0x%08lx virt=%p size=0x%08zx\n",
-		"insert", buf->paddr, buf->vaddr, buf->size);
+	pr_debug("%s: phys=0x%08lx size=0x%08zx\n",
+		 __FUNCTION__, pbuf->paddr, pbuf->size);
 
-	node = rb_find_key_n(virt_rb, buf->vaddr, cmp_virt, &found);
+	node = rb_find_key_n(phys_rb, (void *)pbuf->paddr, cmp_phys, &found);
 	if (found) {
-		struct bmm_buffer *f = rb_val(node);
+		struct bmm_phys_buffer *f = rb_val(node);
 		pr_debug("rb: found: %p\n", f);
-		pr_debug("  p0x%08lx v%p s0x%08zx\n", f->paddr, f->vaddr, f->size);
+		pr_debug("  p0x%08lx s0x%08zx\n", f->paddr, f->size);
 	}
 	assert(found == 0);
-	rb_insert_b(node, buf);
+	rb_insert_b(node, pbuf);
+}
+
+static void bmm_rb_virt_remove(struct bmm_virt_buffer *vbuf)
+{
+	Rb_node node;
+	int found = 0;
+
+	pr_debug("%s: phys=0x%08lx virt=%p size=0x%08zx\n",
+		 __FUNCTION__, vbuf->phys->paddr, vbuf->vaddr, vbuf->size);
 
-	node = rb_find_key_n(phys_rb, (void *)buf->paddr, cmp_phys, &found);
+	node = rb_find_key_n(virt_rb, vbuf->vaddr, cmp_virt, &found);
+	assert(found);
+	rb_delete_node(node);
+}
+
+static void bmm_rb_virt_insert(struct bmm_virt_buffer *vbuf)
+{
+	Rb_node node;
+	int found = 0;
+
+	pr_debug("%s: phys=0x%08lx virt=%p size=0x%08zx\n",
+		 __FUNCTION__, vbuf->phys->paddr, vbuf->vaddr, vbuf->size);
+
+	node = rb_find_key_n(virt_rb, vbuf->vaddr, cmp_virt, &found);
 	if (found) {
-		struct bmm_buffer *f = rb_val(node);
+		struct bmm_virt_buffer *f = rb_val(node);
 		pr_debug("rb: found: %p\n", f);
-		pr_debug("  p0x%08lx v%p s0x%08zx\n", f->paddr, f->vaddr, f->size);
+		pr_debug("  p0x%08lx v%p s0x%08zx\n", f->phys->paddr, f->vaddr, f->size);
 	}
 	assert(found == 0);
-	rb_insert_b(node, buf);
+	rb_insert_b(node, vbuf);
 }
 
 static int bmm_get_api_version(void)
@@ -226,7 +255,8 @@ void bmm_exit(void)
 void *bmm_malloc_aligned_phys(unsigned long size, int attr, unsigned align,
                               unsigned long *paddr)
 {
-	struct bmm_buffer *buf;
+	struct bmm_phys_buffer *pbuf;
+	struct bmm_virt_buffer *vbuf;
 	int ret;
 	void *vaddr;
 	ioctl_arg_t io;
@@ -237,9 +267,15 @@ void *bmm_malloc_aligned_phys(unsigned long size, int attr, unsigned align,
 	if(bmm_init() < 0)
 		return NULL;
 
-	buf = malloc(sizeof(*buf));
-	if (!buf)
+	pbuf = malloc(sizeof(*pbuf));
+	vbuf = malloc(sizeof(*vbuf));
+	if (!pbuf || !vbuf) {
+		if (pbuf)
+			free(pbuf);
+		if (vbuf)
+			free(vbuf);
 		return NULL;
+	}
 
 	pr_debug("%s(size=%lu,attr=%x,align=%u,paddr=%p)\n",
 		 __FUNCTION__, size, attr, align, paddr);
@@ -268,15 +304,33 @@ void *bmm_malloc_aligned_phys(unsigned long size, int attr, unsigned align,
 	if (paddr)
 		*paddr = io.output;
 
-	buf->vaddr = vaddr;
-	buf->paddr = io.output;
-	buf->size = size;
-	buf->attr = attr;
+	pbuf->paddr = io.output;
+	pbuf->size = size;
+	pbuf->attr = attr;
+	pbuf->virt = vbuf;
+	pbuf->attach = 1;
+
+	vbuf->vaddr = vaddr;
+	vbuf->size = size;
+	vbuf->phys = pbuf;
 
 	pthread_mutex_lock(&bmm_mutex);
-	bmm_buf_insert(buf);
+	bmm_rb_phys_insert(pbuf);
+	bmm_rb_virt_insert(vbuf);
 	pthread_mutex_unlock(&bmm_mutex);
 
+	/*
+	 * If we keep bmm buffers across fork, they're reference
+	 * counted, so we can drop the allocation reference here.
+	 * This also "disowns" this buffer from this thread group.
+	 */
+	if (API_FEAT(bmm_api) >= API_FEAT_KEEP_FORK) {
+		io.input = io.output;
+		io.output = 0;
+		io.arg = 0;
+		ioctl(bmm_fd, BMM_FREE_PHYS, &io);
+	}
+
 	return vaddr;
 
  err_free_bmm:
@@ -289,7 +343,8 @@ void *bmm_malloc_aligned_phys(unsigned long size, int attr, unsigned align,
 	}
 
  err_free_buf:
-	free(buf);
+	free(pbuf);
+	free(vbuf);
 	return NULL;
 }
 
@@ -305,38 +360,56 @@ void *bmm_malloc(unsigned long size, int attr)
 
 void bmm_free(void *vaddr)
 {
-	struct bmm_buffer *buf;
+	struct bmm_phys_buffer *pbuf = NULL;
+	struct bmm_virt_buffer *vbuf;
 	ioctl_arg_t io;
 
 	if (bmm_init() < 0)
 		return;
 
 	pthread_mutex_lock(&bmm_mutex);
-	buf = bmm_buf_find_virt(vaddr);
-	if (buf)
-		bmm_buf_remove(buf);
+	vbuf = bmm_buf_find_virt(vaddr);
+	if (vbuf) {
+		pbuf = vbuf->phys;
+		if (pbuf->virt == vbuf)
+			pbuf->virt = NULL;
+		bmm_rb_virt_remove(vbuf);
+		if (--pbuf->attach == 0)
+			bmm_rb_phys_remove(pbuf);
+	}
 	pthread_mutex_unlock(&bmm_mutex);
 
-	assert(buf);
-
-	munmap(buf->vaddr, buf->size);
-
-	if (API_FEAT(bmm_api) >= API_FEAT_FREE_PHYS) {
-		io.input = buf->paddr;
-		io.output = 0;
-		io.arg = 0;
-		ioctl(bmm_fd, BMM_FREE_PHYS, &io);
-	} else {
-		io.input = (unsigned long)buf->vaddr;
-		io.output = 0;
-		io.arg = 0;
-		ioctl(bmm_fd, BMM_FREE, &io);
+	assert(vbuf);
+	assert(pbuf);
+
+	munmap(vbuf->vaddr, vbuf->size);
+
+	/*
+	 * If we keep bmm buffers across fork, they're reference
+	 * counted, so we don't need to free them on the last munmap.
+	 */
+	if (API_FEAT(bmm_api) < API_FEAT_KEEP_FORK) {
+		if (API_FEAT(bmm_api) >= API_FEAT_FREE_PHYS) {
+			io.input = pbuf->paddr;
+			io.output = 0;
+			io.arg = 0;
+			ioctl(bmm_fd, BMM_FREE_PHYS, &io);
+		} else {
+			io.input = (unsigned long)vbuf->vaddr;
+			io.output = 0;
+			io.arg = 0;
+			ioctl(bmm_fd, BMM_FREE, &io);
+		}
 	}
-	free(buf);
+	if (pbuf->attach == 0)
+		free(pbuf);
+	free(vbuf);
 }
 
 void *bmm_attach(unsigned long paddr, unsigned long len)
 {
+	struct bmm_phys_buffer *pbuf, *new_pbuf;
+	struct bmm_virt_buffer *vbuf;
 	void *vaddr;
 
 	if(len == 0)
@@ -345,31 +418,110 @@ void *bmm_attach(unsigned long paddr, unsigned long len)
 	if(bmm_init() < 0)
 		return NULL;
 
+	/* Try to map it */
 	vaddr = mmap(0, len, PROT_READ | PROT_WRITE, MAP_SHARED, bmm_fd, paddr);
+	if (vaddr == (void *)-1)
+		return NULL;
+
+	vbuf = malloc(sizeof(*vbuf));
+	new_pbuf = malloc(sizeof(*new_pbuf));
+	if (!vbuf || !new_pbuf) {
+		if (vbuf)
+			free(vbuf);
+		if (new_pbuf)
+			free(new_pbuf);
+		munmap(vaddr, len);
+		return NULL;
+	}
 
-	return ((int)vaddr == -1) ? NULL : vaddr;
+	vbuf->vaddr = vaddr;
+	vbuf->size = len;
+
+	pthread_mutex_lock(&bmm_mutex);
+	pbuf = bmm_buf_find_phys(paddr);
+	if (pbuf) {
+		/*
+		 * If we find the buffer, it means we already know about
+		 * this buffer.  Increment the number of attachments we
+		 * know about for it, and insert the new virtual buffer.
+		 */
+		pbuf->attach++;
+	} else {
+		/*
+		 * Otherwise, we're importing a new buffer which we know
+		 * nothing about.  Create a new pbuf entry.
+		 */
+		new_pbuf->paddr = paddr;
+		new_pbuf->size = len;
+		new_pbuf->attr = 0;
+		new_pbuf->virt = NULL;
+		new_pbuf->attach = 1;
+		pbuf = new_pbuf;
+
+		bmm_rb_phys_insert(new_pbuf);
+	}
+
+	if (!pbuf->virt)
+		pbuf->virt = vbuf;
+	vbuf->phys = pbuf;
+	bmm_rb_virt_insert(vbuf);
+
+	pthread_mutex_unlock(&bmm_mutex);
+
+	/* If the new pbuf wasn't used, free it. */
+	if (pbuf != new_pbuf)
+		free(new_pbuf);
+
+	return vaddr;
 }
 
 void bmm_detach(void *vaddr, unsigned long len)
 {
+	struct bmm_virt_buffer *vbuf;
+	struct bmm_phys_buffer *pbuf;
+
 	if(bmm_init() < 0)
 		return;
 
-	munmap(vaddr, len);
+	pthread_mutex_lock(&bmm_mutex);
+	vbuf = bmm_buf_find_virt(vaddr);
+	if (vbuf) {
+		pbuf = vbuf->phys;
+		assert(pbuf->attach > 0);
+
+		if (pbuf->virt == vbuf)
+			pbuf->virt = NULL;
+
+		bmm_rb_virt_remove(vbuf);
+
+		if (--pbuf->attach == 0)
+			bmm_rb_phys_remove(pbuf);
+		else
+			pbuf = NULL;
+	} else {
+		pbuf = NULL;
+	}
+	pthread_mutex_unlock(&bmm_mutex);
+
+	munmap(vbuf->vaddr, vbuf->size);
+
+	free(vbuf);
+	if (pbuf)
+		free(pbuf);
 }
 
 void *bmm_get_vaddr(unsigned long paddr)
 {
-	struct bmm_buffer *buf;
+	struct bmm_phys_buffer *pbuf;
 	void *va = NULL;
 
 	if (bmm_init() < 0)
 		return 0;
 
 	pthread_mutex_lock(&bmm_mutex);
-	buf = bmm_buf_find_phys(paddr);
-	if (buf)
-		va = buf->vaddr + (paddr - buf->paddr);
+	pbuf = bmm_buf_find_phys(paddr);
+	if (pbuf && pbuf->virt)
+		va = pbuf->virt->vaddr + (paddr - pbuf->paddr);
 	pthread_mutex_unlock(&bmm_mutex);
 
 	return va;
@@ -377,16 +529,16 @@ void *bmm_get_vaddr(unsigned long paddr)
 
 unsigned long bmm_get_paddr(void *vaddr)
 {
-	struct bmm_buffer *buf;
+	struct bmm_virt_buffer *vbuf;
 	unsigned long pa = 0;
 
 	if (bmm_init() < 0)
 		return 0;
 
 	pthread_mutex_lock(&bmm_mutex);
-	buf = bmm_buf_find_virt(vaddr);
-	if (buf)
-		pa = buf->paddr + (vaddr - buf->vaddr);
+	vbuf = bmm_buf_find_virt(vaddr);
+	if (vbuf)
+		pa = vbuf->phys->paddr + (vaddr - vbuf->vaddr);
 	pthread_mutex_unlock(&bmm_mutex);
 
 	return pa;
@@ -411,16 +563,16 @@ int bmm_get_dmabuf_fd(void *vaddr)
 
 unsigned long bmm_get_mem_size(void *vaddr)
 {
-	struct bmm_buffer *buf;
+	struct bmm_virt_buffer *vbuf;
 	unsigned long size = 0;
 
 	if (bmm_init() < 0)
 		return 0;
 
 	pthread_mutex_lock(&bmm_mutex);
-	buf = bmm_buf_find_virt(vaddr);
-	if (buf)
-		size = buf->size;
+	vbuf = bmm_buf_find_virt(vaddr);
+	if (vbuf)
+		size = vbuf->size;
 	pthread_mutex_unlock(&bmm_mutex);
 
 	return size;
@@ -428,16 +580,16 @@ unsigned long bmm_get_mem_size(void *vaddr)
 
 int bmm_get_mem_attr(void *vaddr)
 {
-	struct bmm_buffer *buf;
+	struct bmm_virt_buffer *vbuf;
 	int attr = 0;
 
 	if (bmm_init() < 0)
 		return 0;
 
 	pthread_mutex_lock(&bmm_mutex);
-	buf = bmm_buf_find_virt(vaddr);
-	if (buf)
-		attr = buf->attr;
+	vbuf = bmm_buf_find_virt(vaddr);
+	if (vbuf)
+		attr = vbuf->phys->attr;
 	pthread_mutex_unlock(&bmm_mutex);
 
 	return attr;
@@ -445,7 +597,7 @@ int bmm_get_mem_attr(void *vaddr)
 
 int bmm_set_mem_attr(void *vaddr, int attr)
 {
-	struct bmm_buffer *buf;
+	struct bmm_virt_buffer *vbuf;
 	int ret;
 	ioctl_arg_t io;
 
@@ -462,9 +614,9 @@ int bmm_set_mem_attr(void *vaddr, int attr)
 	attr = io.output;
 
 	pthread_mutex_lock(&bmm_mutex);
-	buf = bmm_buf_find_virt(vaddr);
-	if (buf)
-		buf->attr = attr;
+	vbuf = bmm_buf_find_virt(vaddr);
+	if (vbuf)
+		vbuf->phys->attr = attr;
 	pthread_mutex_unlock(&bmm_mutex);
 
 	return attr;