1 files changed, 144 insertions, 62 deletions

diff --git a/linux/drivers/media/video/videobuf-vmalloc.c b/linux/drivers/media/video/videobuf-vmalloc.c
index 9e6ff640e..6f487b348 100644
--- a/linux/drivers/media/video/videobuf-vmalloc.c
+++ b/linux/drivers/media/video/videobuf-vmalloc.c
@@ -58,20 +58,26 @@ videobuf_vm_open(struct vm_area_struct *vma)
 	map->count++;
 }
 
-static void
-videobuf_vm_close(struct vm_area_struct *vma)
+static void videobuf_vm_close(struct vm_area_struct *vma)
 {
 	struct videobuf_mapping *map = vma->vm_private_data;
 	struct videobuf_queue *q = map->q;
 	int i;
 
-	dprintk(2,"vm_close %p [count=%u,vma=%08lx-%08lx]\n",map,
-		map->count,vma->vm_start,vma->vm_end);
+	dprintk(2,"vm_close %p [count=%u,vma=%08lx-%08lx]\n", map,
+		map->count, vma->vm_start, vma->vm_end);
 
 	map->count--;
 	if (0 == map->count) {
-		dprintk(1,"munmap %p q=%p\n",map,q);
+		struct videobuf_vmalloc_memory *mem;
+
+		dprintk(1, "munmap %p q=%p\n", map, q);
 		mutex_lock(&q->vb_lock);
+
+		/* We need first to cancel streams, before unmapping */
+		if (q->streaming)
+			videobuf_queue_cancel(q);
+
 		for (i = 0; i < VIDEO_MAX_FRAME; i++) {
 			if (NULL == q->bufs[i])
 				continue;
@@ -79,12 +85,35 @@ videobuf_vm_close(struct vm_area_struct *vma)
 			if (q->bufs[i]->map != map)
 				continue;
 
+			mem = q->bufs[i]->priv;
+			if (mem) {
+				/* This callback is called only if kernel has
+				   allocated memory and this memory is mmapped.
+				   In this case, memory should be freed,
+				   in order to do memory unmap.
+				 */
+
+				MAGIC_CHECK(mem->magic, MAGIC_VMAL_MEM);
+
+				/* vfree is not atomic - can't be
+				   called with IRQ's disabled
+				 */
+				dprintk(1, "%s: buf[%d] freeing (%p)\n",
+					__func__, i, mem->vmalloc);
+
+				vfree(mem->vmalloc);
+				mem->vmalloc = NULL;
+			}
+
 			q->bufs[i]->map   = NULL;
 			q->bufs[i]->baddr = 0;
 		}
-		mutex_unlock(&q->vb_lock);
+
 		kfree(map);
+
+		mutex_unlock(&q->vb_lock);
 	}
+
 	return;
 }
 
@@ -125,53 +154,77 @@ static int __videobuf_iolock (struct videobuf_queue* q,
 			      struct videobuf_buffer *vb,
 			      struct v4l2_framebuffer *fbuf)
 {
+	struct videobuf_vmalloc_memory *mem = vb->priv;
 	int pages;
-	struct videobuf_vmalloc_memory *mem=vb->priv;
 
 	BUG_ON(!mem);
 
-	MAGIC_CHECK(mem->magic,MAGIC_VMAL_MEM);
+	MAGIC_CHECK(mem->magic, MAGIC_VMAL_MEM);
 
-	pages = PAGE_ALIGN(vb->size) >> PAGE_SHIFT;
+	switch (vb->memory) {
+	case V4L2_MEMORY_MMAP:
+		dprintk(1, "%s memory method MMAP\n", __func__);
 
-	/* Currently, doesn't support V4L2_MEMORY_OVERLAY */
-	if ((vb->memory != V4L2_MEMORY_MMAP) &&
-				(vb->memory != V4L2_MEMORY_USERPTR) ) {
-		printk(KERN_ERR "Method currently unsupported.\n");
-		return -EINVAL;
-	}
+		/* All handling should be done by __videobuf_mmap_mapper() */
+		if (!mem->vmalloc) {
+			printk(KERN_ERR "memory is not alloced/mmapped.\n");
+			return -EINVAL;
+		}
+		break;
+	case V4L2_MEMORY_USERPTR:
+		pages = PAGE_ALIGN(vb->size);
 
-	/* FIXME: should be tested with kernel mmap mem */
-	mem->vmalloc=vmalloc_user (PAGE_ALIGN(vb->size));
-	if (NULL == mem->vmalloc) {
-		printk(KERN_ERR "vmalloc (%d pages) failed\n",pages);
-		return -ENOMEM;
-	}
+		dprintk(1, "%s memory method USERPTR\n", __func__);
 
-	dprintk(1,"vmalloc is at addr 0x%08lx, size=%d\n",
-				(unsigned long)mem->vmalloc,
-				pages << PAGE_SHIFT);
+#if 1 /* keep */
+		if (vb->baddr) {
+			printk(KERN_ERR "USERPTR is currently not supported\n");
+			return -EINVAL;
+		}
+#endif
 
-	return 0;
-}
+		/* The only USERPTR currently supported is the one needed for
+		   read() method.
+		 */
 
-static int __videobuf_mmap_setup(struct videobuf_queue *q,
-			      struct videobuf_buffer *vb)
-{
-	int retval = 0;
-	BUG_ON(vb->memory != V4L2_MEMORY_MMAP);
-	if (vb->state == VIDEOBUF_NEEDS_INIT) {
-		/* bsize == size since the buffer needs to be large enough to
-		 * hold an entire frame, not the case in the read case for
-		 * example*/
-		vb->size = vb->bsize;
-		retval = __videobuf_iolock(q, vb, NULL);
-		if (!retval) {
-			/* Don't IOLOCK later */
-			vb->state = VIDEOBUF_IDLE;
+		mem->vmalloc = vmalloc_user(pages);
+		if (!mem->vmalloc) {
+			printk(KERN_ERR "vmalloc (%d pages) failed\n", pages);
+			return -ENOMEM;
+		}
+		dprintk(1, "vmalloc is at addr %p (%d pages)\n",
+			mem->vmalloc, pages);
+
+#if 0 /* keep */
+		int rc;
+		/* Kernel userptr is used also by read() method. In this case,
+		   there's no need to remap, since data will be copied to user
+		 */
+		if (!vb->baddr)
+			return 0;
+
+		/* FIXME: to properly support USERPTR, remap should occur.
+		   The code bellow won't work, since mem->vma = NULL
+		 */
+		/* Try to remap memory */
+		rc = remap_vmalloc_range(mem->vma, (void *)vb->baddr, 0);
+		if (rc < 0) {
+			printk(KERN_ERR "mmap: remap failed with error %d. ", rc);
+			return -ENOMEM;
 		}
+#endif
+
+		break;
+	case V4L2_MEMORY_OVERLAY:
+	default:
+		dprintk(1, "%s memory method OVERLAY/unknown\n", __func__);
+
+		/* Currently, doesn't support V4L2_MEMORY_OVERLAY */
+		printk(KERN_ERR "Memory method currently unsupported.\n");
+		return -EINVAL;
 	}
-	return retval;
+
+	return 0;
 }
 
 static int __videobuf_sync(struct videobuf_queue *q,
@@ -184,6 +237,7 @@ static int __videobuf_mmap_free(struct videobuf_queue *q)
 {
 	unsigned int i;
 
+	dprintk(1, "%s\n", __func__);
 	for (i = 0; i < VIDEO_MAX_FRAME; i++) {
 		if (q->bufs[i]) {
 			if (q->bufs[i]->map)
@@ -200,10 +254,11 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	struct videobuf_vmalloc_memory *mem;
 	struct videobuf_mapping *map;
 	unsigned int first;
-	int retval;
+	int retval, pages;
 	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
 
-	if (! (vma->vm_flags & VM_WRITE) || ! (vma->vm_flags & VM_SHARED))
+	dprintk(1, "%s\n", __func__);
+	if (!(vma->vm_flags & VM_WRITE) || !(vma->vm_flags & VM_SHARED))
 		return -EINVAL;
 
 	/* look for first buffer to map */
@@ -223,39 +278,55 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	}
 
 	/* create mapping + update buffer list */
-	map = q->bufs[first]->map = kzalloc(sizeof(struct videobuf_mapping),GFP_KERNEL);
+	map = kzalloc(sizeof(struct videobuf_mapping), GFP_KERNEL);
 	if (NULL == map)
 		return -ENOMEM;
 
+	q->bufs[first]->map = map;
 	map->start = vma->vm_start;
 	map->end   = vma->vm_end;
 	map->q     = q;
 
 	q->bufs[first]->baddr = vma->vm_start;
 
-	vma->vm_ops          = &videobuf_vm_ops;
-	vma->vm_flags       |= VM_DONTEXPAND | VM_RESERVED;
-	vma->vm_private_data = map;
+	mem = q->bufs[first]->priv;
+	BUG_ON(!mem);
+	MAGIC_CHECK(mem->magic, MAGIC_VMAL_MEM);
 
-	mem=q->bufs[first]->priv;
-	BUG_ON (!mem);
-	MAGIC_CHECK(mem->magic,MAGIC_VMAL_MEM);
+	pages = PAGE_ALIGN(vma->vm_end - vma->vm_start);
+	mem->vmalloc = vmalloc_user(pages);
+	if (!mem->vmalloc) {
+		printk(KERN_ERR "vmalloc (%d pages) failed\n", pages);
+		goto error;
+	}
+	dprintk(1, "vmalloc is at addr %p (%d pages)\n",
+		mem->vmalloc, pages);
 
 	/* Try to remap memory */
-	retval=remap_vmalloc_range(vma, mem->vmalloc,0);
-	if (retval<0) {
-		dprintk(1, "mmap: failed to remap_vmalloc_range\n");
-		return -EINVAL;
+	retval = remap_vmalloc_range(vma, mem->vmalloc, 0);
+	if (retval < 0) {
+		printk(KERN_ERR "mmap: remap failed with error %d. ", retval);
+		vfree(mem->vmalloc);
+		goto error;
 	}
 
+	vma->vm_ops          = &videobuf_vm_ops;
+	vma->vm_flags       |= VM_DONTEXPAND | VM_RESERVED;
+	vma->vm_private_data = map;
+
 	dprintk(1,"mmap %p: q=%p %08lx-%08lx (%lx) pgoff %08lx buf %d\n",
-		map,q,vma->vm_start,vma->vm_end,
+		map, q, vma->vm_start, vma->vm_end,
 		(long int) q->bufs[first]->bsize,
-		vma->vm_pgoff,first);
+		vma->vm_pgoff, first);
 
 	videobuf_vm_open(vma);
 
-	return (0);
+	return 0;
+
+error:
+	mem = NULL;
+	kfree(map);
+	return -ENOMEM;
 }
 
 static int __videobuf_copy_to_user ( struct videobuf_queue *q,
@@ -313,11 +384,11 @@ static struct videobuf_qtype_ops qops = {
 	.alloc        = __videobuf_alloc,
 	.iolock       = __videobuf_iolock,
 	.sync         = __videobuf_sync,
-	.mmap_setup   = __videobuf_mmap_setup,
 	.mmap_free    = __videobuf_mmap_free,
 	.mmap_mapper  = __videobuf_mmap_mapper,
 	.video_copy_to_user = __videobuf_copy_to_user,
 	.copy_stream  = __videobuf_copy_stream,
+	.vmalloc      = videobuf_to_vmalloc,
 };
 
 void videobuf_queue_vmalloc_init(struct videobuf_queue* q,
@@ -347,13 +418,24 @@ EXPORT_SYMBOL_GPL(videobuf_to_vmalloc);
 
 void videobuf_vmalloc_free (struct videobuf_buffer *buf)
 {
-	struct videobuf_vmalloc_memory *mem=buf->priv;
-	BUG_ON (!mem);
+	struct videobuf_vmalloc_memory *mem = buf->priv;
 
-	MAGIC_CHECK(mem->magic,MAGIC_VMAL_MEM);
+	/* mmapped memory can't be freed here, otherwise mmapped region
+	   would be released, while still needed. In this case, the memory
+	   release should happen inside videobuf_vm_close().
+	   So, it should free memory only if the memory were allocated for
+	   read() operation.
+	 */
+	if ((buf->memory != V4L2_MEMORY_USERPTR) || (buf->baddr == 0))
+		return;
+
+	if (!mem)
+		return;
+
+	MAGIC_CHECK(mem->magic, MAGIC_VMAL_MEM);
 
 	vfree(mem->vmalloc);
-	mem->vmalloc=NULL;
+	mem->vmalloc = NULL;
 
 	return;
 }