1 files changed, 58 insertions, 4 deletions

diff --git a/drivers/media/v4l2-core/videobuf-dma-sg.c b/drivers/media/v4l2-core/videobuf-dma-sg.c
index 828e7c10bd70..3c8cc023a5a5 100644
--- a/drivers/media/v4l2-core/videobuf-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf-dma-sg.c
@@ -211,13 +211,36 @@ EXPORT_SYMBOL_GPL(videobuf_dma_init_user);
 int videobuf_dma_init_kernel(struct videobuf_dmabuf *dma, int direction,
 			     int nr_pages)
 {
+	int i;
+
 	dprintk(1, "init kernel [%d pages]\n", nr_pages);
 
 	dma->direction = direction;
-	dma->vaddr = vmalloc_32(nr_pages << PAGE_SHIFT);
+	dma->vaddr_pages = kcalloc(nr_pages, sizeof(*dma->vaddr_pages),
+				   GFP_KERNEL);
+	if (!dma->vaddr_pages)
+		return -ENOMEM;
+
+	dma->dma_addr = kcalloc(nr_pages, sizeof(*dma->dma_addr), GFP_KERNEL);
+	if (!dma->dma_addr) {
+		kfree(dma->vaddr_pages);
+		return -ENOMEM;
+	}
+	for (i = 0; i < nr_pages; i++) {
+		void *addr;
+
+		addr = dma_alloc_coherent(dma->dev, PAGE_SIZE,
+					  &(dma->dma_addr[i]), GFP_KERNEL);
+		if (addr == NULL)
+			goto out_free_pages;
+
+		dma->vaddr_pages[i] = virt_to_page(addr);
+	}
+	dma->vaddr = vmap(dma->vaddr_pages, nr_pages, VM_MAP | VM_IOREMAP,
+			  PAGE_KERNEL);
 	if (NULL == dma->vaddr) {
 		dprintk(1, "vmalloc_32(%d pages) failed\n", nr_pages);
-		return -ENOMEM;
+		goto out_free_pages;
 	}
 
 	dprintk(1, "vmalloc is at addr 0x%08lx, size=%d\n",
@@ -228,6 +251,19 @@ int videobuf_dma_init_kernel(struct videobuf_dmabuf *dma, int direction,
 	dma->nr_pages = nr_pages;
 
 	return 0;
+out_free_pages:
+	while (i > 0) {
+		void *addr = page_address(dma->vaddr_pages[i]);
+		dma_free_coherent(dma->dev, PAGE_SIZE, addr, dma->dma_addr[i]);
+		i--;
+	}
+	kfree(dma->dma_addr);
+	dma->dma_addr = NULL;
+	kfree(dma->vaddr_pages);
+	dma->vaddr_pages = NULL;
+
+	return -ENOMEM;
+
 }
 EXPORT_SYMBOL_GPL(videobuf_dma_init_kernel);
 
@@ -322,8 +358,21 @@ int videobuf_dma_free(struct videobuf_dmabuf *dma)
 		dma->pages = NULL;
 	}
 
-	vfree(dma->vaddr);
-	dma->vaddr = NULL;
+	if (dma->dma_addr) {
+		for (i = 0; i < dma->nr_pages; i++) {
+			void *addr;
+
+			addr = page_address(dma->vaddr_pages[i]);
+			dma_free_coherent(dma->dev, PAGE_SIZE, addr,
+					  dma->dma_addr[i]);
+		}
+		kfree(dma->dma_addr);
+		dma->dma_addr = NULL;
+		kfree(dma->vaddr_pages);
+		dma->vaddr_pages = NULL;
+		vunmap(dma->vaddr);
+		dma->vaddr = NULL;
+	}
 
 	if (dma->bus_addr)
 		dma->bus_addr = 0;
@@ -461,6 +510,11 @@ static int __videobuf_iolock(struct videobuf_queue *q,
 
 	MAGIC_CHECK(mem->magic, MAGIC_SG_MEM);
 
+	if (!mem->dma.dev)
+		mem->dma.dev = q->dev;
+	else
+		WARN_ON(mem->dma.dev != q->dev);
+
 	switch (vb->memory) {
 	case V4L2_MEMORY_MMAP:
 	case V4L2_MEMORY_USERPTR: