1 files changed, 318 insertions, 16 deletions

diff --git a/drivers/media/video/samsung/fimc/fimc_dev.c b/drivers/media/video/samsung/fimc/fimc_dev.c
index a0a91cd..1ed79dd 100644
--- a/drivers/media/video/samsung/fimc/fimc_dev.c
+++ b/drivers/media/video/samsung/fimc/fimc_dev.c
@@ -32,9 +32,12 @@
 #include <linux/videodev2_exynos_camera.h>
 #include <linux/delay.h>
 #include <linux/cma.h>
+#include <linux/dma-mapping.h>
 #include <plat/fimc.h>
 #include <plat/clock.h>
 #include <mach/regs-pmu.h>
+#include <linux/cpufreq.h>
+#include <mach/cpufreq.h>
 
 #include "fimc.h"
 
@@ -711,20 +714,32 @@ static struct fimc_control *fimc_register_controller(struct platform_device *pde
 	/* In Midas project, FIMC2 reserve memory is used by ION driver. */
 	if (id != 2) {
 #endif
-		sprintf(ctrl->cma_name, "%s%d", FIMC_CMA_NAME, ctrl->id);
-		err = cma_info(&mem_info, ctrl->dev, 0);
-		fimc_info1("%s : [cma_info] start_addr : 0x%x, end_addr : 0x%x, "
-				"total_size : 0x%x, free_size : 0x%x\n",
-				__func__, mem_info.lower_bound, mem_info.upper_bound,
-				mem_info.total_size, mem_info.free_size);
-		if (err) {
-			fimc_err("%s: get cma info failed\n", __func__);
-			ctrl->mem.size = 0;
+#ifdef CONFIG_USE_FIMC_CMA
+		if (id == 1) {
+			ctrl->mem.size =
+				CONFIG_VIDEO_SAMSUNG_MEMSIZE_FIMC1 * SZ_1K;
 			ctrl->mem.base = 0;
-		} else {
-			ctrl->mem.size = mem_info.total_size;
-			ctrl->mem.base = (dma_addr_t)cma_alloc
-				(ctrl->dev, ctrl->cma_name, (size_t)ctrl->mem.size, 0);
+		} else
+#endif
+		{
+			sprintf(ctrl->cma_name, "%s%d",
+					FIMC_CMA_NAME, ctrl->id);
+			err = cma_info(&mem_info, ctrl->dev, 0);
+			fimc_info1("%s : [cma_info] start_addr : 0x%x, "
+				" end_addr : 0x%x, total_size : 0x%x, "
+				"free_size : 0x%x\n", __func__,
+				mem_info.lower_bound, mem_info.upper_bound,
+				mem_info.total_size, mem_info.free_size);
+			if (err) {
+				fimc_err("%s: get cma info failed\n", __func__);
+				ctrl->mem.size = 0;
+				ctrl->mem.base = 0;
+			} else {
+				ctrl->mem.size = mem_info.total_size;
+				ctrl->mem.base = (dma_addr_t)cma_alloc
+					(ctrl->dev, ctrl->cma_name,
+					(size_t)ctrl->mem.size, 0);
+			}
 		}
 #ifdef CONFIG_ION_EXYNOS
 	}
@@ -813,7 +828,7 @@ static struct fimc_control *fimc_register_controller(struct platform_device *pde
 		clk_put(fimc_src_clk);
 		return NULL;
 	}
-	clk_set_rate(sclk_fimc_lclk, FIMC_CLK_RATE);
+	clk_set_rate(sclk_fimc_lclk, fimc_clk_rate());
 	clk_put(sclk_fimc_lclk);
 	clk_put(fimc_src_clk);
 
@@ -1021,6 +1036,255 @@ static int fimc_mmap(struct file *filp, struct vm_area_struct *vma)
 	return ret;
 }
 
+#ifdef CONFIG_SLP_DMABUF
+/**
+ * _fimc_dmabuf_put() - release memory associated with
+ * a DMABUF shared buffer
+ */
+static void _fimc_dmabuf_put(struct vb2_buffer *vb)
+{
+	unsigned int plane;
+
+	for (plane = 0; plane < vb->num_planes; ++plane) {
+		void *mem_priv = vb->planes[plane].mem_priv;
+
+		if (mem_priv) {
+			dma_buf_detach(vb->planes[plane].dbuf,
+					vb->planes[plane].mem_priv);
+			dma_buf_put(vb->planes[plane].dbuf);
+			vb->planes[plane].dbuf = NULL;
+			vb->planes[plane].mem_priv = NULL;
+		}
+	}
+}
+
+void _fimc_queue_free(struct fimc_control *ctrl, enum v4l2_buf_type type)
+{
+	unsigned int buffer;
+	struct vb2_buffer *vb;
+
+	for (buffer = 0; buffer < VIDEO_MAX_FRAME; ++buffer) {
+		if (V4L2_TYPE_IS_OUTPUT(type))
+			vb = ctrl->out_bufs[buffer];
+		else
+			vb = ctrl->cap_bufs[buffer];
+
+		if (!vb)
+			continue;
+		_fimc_dmabuf_put(vb);
+		kfree(vb);
+		vb = NULL;
+	}
+}
+
+int fimc_queue_alloc(struct fimc_control *ctrl, enum v4l2_buf_type type,
+		enum v4l2_memory memory, unsigned int num_buffers,
+		unsigned int num_planes)
+{
+	unsigned int buffer;
+	struct vb2_buffer *vb;
+
+	for (buffer = 0; buffer < num_buffers; ++buffer) {
+		vb = kzalloc(sizeof(struct vb2_buffer), GFP_KERNEL);
+		if (!vb) {
+			fimc_err("%s: Memory alloc for buffer struct failed\n",
+				__func__);
+			break;
+		}
+
+		if (V4L2_TYPE_IS_MULTIPLANAR(type))
+			vb->v4l2_buf.length = num_planes;
+
+		vb->num_planes = num_planes;
+		vb->v4l2_buf.index = buffer;
+		vb->v4l2_buf.type = type;
+		vb->v4l2_buf.memory = memory;
+
+		if (V4L2_TYPE_IS_OUTPUT(type))
+			ctrl->out_bufs[buffer] = vb;
+		else
+			ctrl->cap_bufs[buffer] = vb;
+	}
+
+	for (buffer = num_buffers; buffer < VIDEO_MAX_FRAME; ++buffer) {
+		ctrl->out_bufs[buffer] = NULL;
+		ctrl->cap_bufs[buffer] = NULL;
+	}
+
+	return buffer;
+}
+
+static inline int _verify_planes_array(struct vb2_buffer *vb,
+				const struct v4l2_buffer *b)
+{
+	if (NULL == b->m.planes) {
+		printk(KERN_ERR "%s: Multi-planar buffer passwd but planes"
+			" array not provided\n", __func__);
+		return -EINVAL;
+	}
+	if (b->length < vb->num_planes || b->length > VIDEO_MAX_PLANES) {
+		printk(KERN_ERR "%s: Incorrect planes array length, "
+			"expected %d, got %d\n", __func__,
+			vb->num_planes, b->length);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int _fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b,
+			struct v4l2_plane *v4l2_planes)
+{
+	int plane;
+	int ret;
+
+	memcpy(b, &vb->v4l2_buf, offsetof(struct v4l2_buffer, m));
+	b->input = vb->v4l2_buf.input;
+	b->reserved = vb->v4l2_buf.reserved;
+
+	if (V4L2_TYPE_IS_MULTIPLANAR(b->type)) {
+		ret = _verify_planes_array(vb, b);
+		if (ret)
+			return ret;
+
+		memcpy(b->m.planes, vb->v4l2_planes,
+			b->length * sizeof(struct v4l2_plane));
+
+		for (plane = 0; plane < vb->num_planes; ++plane)
+			b->m.planes[plane].m.fd =
+				vb->v4l2_planes[plane].m.fd;
+	} else {
+		b->length = vb->v4l2_planes[0].length;
+		b->bytesused = vb->v4l2_planes[0].bytesused;
+		b->m.fd = vb->v4l2_planes[0].m.fd;
+	}
+
+	return ret;
+}
+
+static int _fill_vb2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b,
+			struct v4l2_plane *v4l2_planes)
+{
+	unsigned int plane;
+	int ret;
+
+	if (V4L2_TYPE_IS_MULTIPLANAR(b->type)) {
+		ret = _verify_planes_array(vb, b);
+		if (ret)
+			return ret;
+
+		if (V4L2_TYPE_IS_OUTPUT(b->type)) {
+			for (plane = 0; plane < vb->num_planes; ++plane) {
+				v4l2_planes[plane].bytesused =
+					b->m.planes[plane].bytesused;
+				v4l2_planes[plane].data_offset =
+					b->m.planes[plane].data_offset;
+			}
+		}
+		for (plane = 0; plane < vb->num_planes; ++plane)
+			v4l2_planes[plane].m.fd =
+				b->m.planes[plane].m.fd;
+
+	} else {
+		if (V4L2_TYPE_IS_OUTPUT(b->type))
+			v4l2_planes[0].bytesused =
+					b->bytesused;
+			v4l2_planes[0].m.fd = b->m.fd;
+	}
+
+	vb->v4l2_buf.field = b->field;
+	vb->v4l2_buf.timestamp = b->timestamp;
+	vb->v4l2_buf.input = b->input;
+
+	return 0;
+}
+
+int _qbuf_dmabuf(struct fimc_control *ctrl, struct vb2_buffer *vb,
+	struct v4l2_buffer *b)
+{
+	struct v4l2_plane planes[VIDEO_MAX_PLANES];
+	unsigned int plane;
+	struct sg_table *sg;
+	struct dma_buf_attachment *dba;
+	int ret;
+
+	ret = _fill_vb2_buffer(vb, b, planes);
+	if (ret)
+		return ret;
+
+	for (plane = 0; plane < vb->num_planes; ++plane) {
+		struct dma_buf *dbuf = dma_buf_get(planes[plane].m.fd);
+
+		if (IS_ERR_OR_NULL(dbuf)) {
+			fimc_err("dmabuf get error!!! %x\n", plane);
+			ret = PTR_ERR(dbuf);
+			goto err;
+		}
+		planes[plane].length = dbuf->size;
+
+		/* Skip the plane if already verified */
+		if (dbuf == vb->planes[plane].dbuf) {
+			dma_buf_put(dbuf);
+			continue;
+		}
+
+		vb->planes[plane].mem_priv = NULL;
+
+		dba = dma_buf_attach(dbuf, ctrl->dev);
+		if (IS_ERR(dba)) {
+			fimc_err("failed to attach dmabuf\n");
+			dma_buf_put(dbuf);
+			ret = PTR_ERR(dba);
+			goto err;
+		}
+
+		sg = dma_buf_map_attachment(dba, DMA_BIDIRECTIONAL);
+		if (IS_ERR(sg)) {
+			fimc_err("qbuf: failed acquiring dmabuf "
+					"memory for plane\n");
+			ret = PTR_ERR(sg);
+			goto err;
+		}
+		dba->priv = sg;
+
+		vb->planes[plane].dbuf = dbuf;
+		vb->planes[plane].mem_priv = dba;
+
+		vb->v4l2_planes[plane] = planes[plane];
+	}
+
+	return 0;
+
+err:
+	_fimc_dmabuf_put(vb);
+
+	return ret;
+}
+
+int _dqbuf_dmabuf(struct fimc_control *ctrl, struct vb2_buffer *vb,
+		struct v4l2_buffer *b)
+{
+	struct v4l2_plane planes[VIDEO_MAX_PLANES];
+	struct sg_table *sg[VIDEO_MAX_PLANES];
+	struct dma_buf_attachment *dba[VIDEO_MAX_PLANES];
+	unsigned int plane;
+	int ret;
+
+	ret = _fill_v4l2_buffer(vb, b, planes);
+	if (ret)
+		return ret;
+
+	for (plane = 0; plane < vb->num_planes; ++plane) {
+		dba[plane] = vb->planes[plane].mem_priv;
+		sg[plane] = dba[plane]->priv;
+		dma_buf_unmap_attachment(dba[plane],
+			sg[plane], DMA_FROM_DEVICE);
+	}
+
+	return 0;
+}
+#endif
+
 static u32 fimc_poll(struct file *filp, poll_table *wait)
 {
 	struct fimc_prv_data *prv_data =
@@ -1226,6 +1490,19 @@ static int fimc_open(struct file *filp)
 		goto kzalloc_err;
 	}
 
+#ifdef CONFIG_USE_FIMC_CMA
+	if (ctrl->id == 1) {
+		ctrl->mem.cpu_addr = dma_alloc_coherent(ctrl->dev,
+					ctrl->mem.size, &(ctrl->mem.base), 0);
+		if (!ctrl->mem.cpu_addr) {
+			printk(KERN_INFO "FIMC%d: dma_alloc_coherent failed\n",
+								ctrl->id);
+			ret = -ENOMEM;
+			goto dma_alloc_err;
+		}
+	}
+#endif
+
 	if (in_use == 1) {
 #if (!defined(CONFIG_EXYNOS_DEV_PD) || !defined(CONFIG_PM_RUNTIME))
 		if (pdata->clk_on)
@@ -1276,6 +1553,11 @@ static int fimc_open(struct file *filp)
 
 	return 0;
 
+#ifdef CONFIG_USE_FIMC_CMA
+dma_alloc_err:
+	kfree(prv_data);
+#endif
+
 kzalloc_err:
 	atomic_dec(&ctrl->in_use);
 
@@ -1425,6 +1707,9 @@ static int fimc_release(struct file *filp)
 		} else {
 			ctrl->out->ctx_used[ctx_id] = false;
 		}
+#ifdef CONFIG_SLP_DMABUF
+		_fimc_queue_free(ctrl, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+#endif
 	}
 
 	if (ctrl->cap) {
@@ -1445,6 +1730,9 @@ static int fimc_release(struct file *filp)
 		}
 		kfree(ctrl->cap);
 		ctrl->cap = NULL;
+#ifdef CONFIG_SLP_DMABUF
+		_fimc_queue_free(ctrl, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+#endif
 	}
 
 	/*
@@ -1462,6 +1750,14 @@ static int fimc_release(struct file *filp)
 		ctrl->fb.is_enable = 0;
 	}
 
+#ifdef CONFIG_USE_FIMC_CMA
+	if (ctrl->id == 1) {
+		dma_free_coherent(ctrl->dev, ctrl->mem.size, ctrl->mem.cpu_addr,
+					ctrl->mem.base);
+		ctrl->mem.base = 0;
+		ctrl->mem.cpu_addr = NULL;
+	}
+#endif
 	fimc_warn("FIMC%d %d released.\n",
 			ctrl->id, atomic_read(&ctrl->in_use));
 
@@ -1840,7 +2136,7 @@ static int __devinit fimc_probe(struct platform_device *pdev)
 	ret = device_create_file(&(pdev->dev), &dev_attr_range_mode);
 	if (ret < 0) {
 		fimc_err("failed to add sysfs entries for range mode\n");
-		goto err_global;
+		goto err_create_file;
 	}
 	printk(KERN_INFO "FIMC%d registered successfully\n", ctrl->id);
 #if (defined(CONFIG_EXYNOS_DEV_PD) && defined(CONFIG_PM_RUNTIME))
@@ -1848,7 +2144,7 @@ static int __devinit fimc_probe(struct platform_device *pdev)
 	ctrl->fimc_irq_wq = create_workqueue(buf);
 	if (ctrl->fimc_irq_wq == NULL) {
 		fimc_err("failed to create_workqueue\n");
-		goto err_global;
+		goto err_wq;
 	}
 
 	INIT_WORK(&ctrl->work_struct, s3c_fimc_irq_work);
@@ -1869,6 +2165,12 @@ static int __devinit fimc_probe(struct platform_device *pdev)
 
 	return 0;
 
+err_wq:
+	 device_remove_file(&(pdev->dev), &dev_attr_range_mode);
+
+err_create_file:
+	 device_remove_file(&(pdev->dev), &dev_attr_log_level);
+
 err_global:
 	video_unregister_device(ctrl->vd);