M7350v1_en_gpl

kernel/drivers/media/platform/msm/camera_v2/isp/Makefile

@@ -0,0 +1,4 @@
+ccflags-y += -Idrivers/media/platform/msm/camera_v2
+ccflags-y += -Idrivers/media/platform/msm/camera_v2/sensor/io
+obj-$(CONFIG_MSMB_CAMERA) += msm_isp.o msm_buf_mgr.o msm_isp_util.o msm_isp_axi_util.o msm_isp_stats_util.o
+obj-$(CONFIG_MSMB_CAMERA) += msm_isp40.o msm_isp32.o

kernel/drivers/media/platform/msm/camera_v2/isp/msm_buf_mgr.c

@@ -0,0 +1,838 @@
+/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/workqueue.h>
+#include <linux/delay.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/ioctl.h>
+#include <linux/spinlock.h>
+#include <linux/videodev2.h>
+#include <linux/proc_fs.h>
+#include <linux/videodev2.h>
+#include <linux/vmalloc.h>
+
+
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-device.h>
+#include <media/videobuf2-core.h>
+#include <media/msm_camera.h>
+#include <media/msm_isp.h>
+
+#include <mach/iommu.h>
+
+#include "msm.h"
+#include "msm_buf_mgr.h"
+
+/*#define CONFIG_MSM_ISP_DBG*/
+#undef CDBG
+#ifdef CONFIG_MSM_ISP_DBG
+#define CDBG(fmt, args...) pr_err(fmt, ##args)
+#else
+#define CDBG(fmt, args...) do { } while (0)
+#endif
+
+static struct msm_isp_bufq *msm_isp_get_bufq(
+	struct msm_isp_buf_mgr *buf_mgr,
+	uint32_t bufq_handle)
+{
+	struct msm_isp_bufq *bufq = NULL;
+	uint32_t bufq_index = bufq_handle & 0xFF;
+	if (bufq_index > buf_mgr->num_buf_q)
+		return bufq;
+
+	bufq = &buf_mgr->bufq[bufq_index];
+	if (bufq->bufq_handle == bufq_handle)
+		return bufq;
+
+	return NULL;
+}
+
+static struct msm_isp_buffer *msm_isp_get_buf_ptr(
+	struct msm_isp_buf_mgr *buf_mgr,
+	uint32_t bufq_handle, uint32_t buf_index)
+{
+	struct msm_isp_bufq *bufq = NULL;
+	struct msm_isp_buffer *buf_info = NULL;
+
+	bufq = msm_isp_get_bufq(buf_mgr, bufq_handle);
+	if (!bufq) {
+		pr_err("%s: Invalid bufq\n", __func__);
+		return buf_info;
+	}
+
+	if (bufq->num_bufs <= buf_index) {
+		pr_err("%s: Invalid buf index\n", __func__);
+		return buf_info;
+	}
+	buf_info = &bufq->bufs[buf_index];
+	return buf_info;
+}
+
+static uint32_t msm_isp_get_buf_handle(
+	struct msm_isp_buf_mgr *buf_mgr,
+	uint32_t session_id, uint32_t stream_id)
+{
+	int i;
+	if ((buf_mgr->buf_handle_cnt << 8) == 0)
+		buf_mgr->buf_handle_cnt++;
+
+	for (i = 0; i < buf_mgr->num_buf_q; i++) {
+		if (buf_mgr->bufq[i].session_id == session_id &&
+			buf_mgr->bufq[i].stream_id == stream_id)
+			return 0;
+	}
+
+	for (i = 0; i < buf_mgr->num_buf_q; i++) {
+		if (buf_mgr->bufq[i].bufq_handle == 0) {
+			memset(&buf_mgr->bufq[i],
+				0, sizeof(struct msm_isp_bufq));
+			buf_mgr->bufq[i].bufq_handle =
+				(++buf_mgr->buf_handle_cnt) << 8 | i;
+			return buf_mgr->bufq[i].bufq_handle;
+		}
+	}
+	return 0;
+}
+
+static int msm_isp_free_buf_handle(struct msm_isp_buf_mgr *buf_mgr,
+	uint32_t bufq_handle)
+{
+	struct msm_isp_bufq *bufq =
+		msm_isp_get_bufq(buf_mgr, bufq_handle);
+	if (!bufq)
+		return -EINVAL;
+	memset(bufq, 0, sizeof(struct msm_isp_bufq));
+	return 0;
+}
+
+static int msm_isp_prepare_v4l2_buf(struct msm_isp_buf_mgr *buf_mgr,
+	struct msm_isp_buffer *buf_info,
+	struct v4l2_buffer *v4l2_buf)
+{
+	int i, rc = -1;
+	struct msm_isp_buffer_mapped_info *mapped_info;
+	for (i = 0; i < v4l2_buf->length; i++) {
+		mapped_info = &buf_info->mapped_info[i];
+		mapped_info->handle =
+		ion_import_dma_buf(buf_mgr->client,
+			v4l2_buf->m.planes[i].m.userptr);
+		if (IS_ERR_OR_NULL(mapped_info->handle)) {
+			pr_err("%s: buf has null/error ION handle %p\n",
+				__func__, mapped_info->handle);
+			goto ion_map_error;
+		}
+		if (ion_map_iommu(buf_mgr->client, mapped_info->handle,
+				buf_mgr->iommu_domain_num, 0, SZ_4K,
+				0, &(mapped_info->paddr),
+				&(mapped_info->len), 0, 0) < 0) {
+			rc = -EINVAL;
+			pr_err("%s: cannot map address", __func__);
+			ion_free(buf_mgr->client, mapped_info->handle);
+			goto ion_map_error;
+		}
+		mapped_info->paddr += v4l2_buf->m.planes[i].data_offset;
+		CDBG("%s: plane: %d addr:%lu\n",
+			__func__, i, mapped_info->paddr);
+	}
+	buf_info->num_planes = v4l2_buf->length;
+	return 0;
+ion_map_error:
+	for (--i; i >= 0; i--) {
+		mapped_info = &buf_info->mapped_info[i];
+		ion_unmap_iommu(buf_mgr->client, mapped_info->handle,
+		buf_mgr->iommu_domain_num, 0);
+		ion_free(buf_mgr->client, mapped_info->handle);
+	}
+	return rc;
+}
+
+static void msm_isp_unprepare_v4l2_buf(
+	struct msm_isp_buf_mgr *buf_mgr,
+	struct msm_isp_buffer *buf_info)
+{
+	int i;
+	struct msm_isp_buffer_mapped_info *mapped_info;
+	for (i = 0; i < buf_info->num_planes; i++) {
+		mapped_info = &buf_info->mapped_info[i];
+		ion_unmap_iommu(buf_mgr->client, mapped_info->handle,
+			buf_mgr->iommu_domain_num, 0);
+		ion_free(buf_mgr->client, mapped_info->handle);
+	}
+	return;
+}
+
+static int msm_isp_buf_prepare(struct msm_isp_buf_mgr *buf_mgr,
+	struct msm_isp_qbuf_info *info, struct vb2_buffer *vb2_buf)
+{
+	int rc = -1;
+	unsigned long flags;
+	struct msm_isp_bufq *bufq = NULL;
+	struct msm_isp_buffer *buf_info = NULL;
+	struct v4l2_buffer *buf = NULL;
+	struct v4l2_plane *plane = NULL;
+
+	buf_info = msm_isp_get_buf_ptr(buf_mgr,
+		info->handle, info->buf_idx);
+	if (!buf_info) {
+		pr_err("Invalid buffer prepare\n");
+		return rc;
+	}
+
+	bufq = msm_isp_get_bufq(buf_mgr, buf_info->bufq_handle);
+	if (!bufq) {
+		pr_err("%s: Invalid bufq\n", __func__);
+		return rc;
+	}
+
+	spin_lock_irqsave(&bufq->bufq_lock, flags);
+	if (buf_info->state == MSM_ISP_BUFFER_STATE_DIVERTED) {
+		rc = buf_info->state;
+		spin_unlock_irqrestore(&bufq->bufq_lock, flags);
+		return rc;
+	}
+
+	if (buf_info->state != MSM_ISP_BUFFER_STATE_INITIALIZED) {
+		pr_err("%s: Invalid buffer state: %d\n",
+			__func__, buf_info->state);
+		spin_unlock_irqrestore(&bufq->bufq_lock, flags);
+		return rc;
+	}
+	spin_unlock_irqrestore(&bufq->bufq_lock, flags);
+
+	if (vb2_buf) {
+		buf = &vb2_buf->v4l2_buf;
+		buf_info->vb2_buf = vb2_buf;
+	} else {
+		buf = &info->buffer;
+		plane =
+			kzalloc(sizeof(struct v4l2_plane) * buf->length,
+				GFP_KERNEL);
+		if (!plane) {
+			pr_err("%s: Cannot alloc plane: %d\n",
+			__func__, buf_info->state);
+			return rc;
+		}
+		if (copy_from_user(plane,
+				(void __user *)(buf->m.planes),
+			sizeof(struct v4l2_plane) * buf->length)) {
+			kfree(plane);
+			return rc;
+		}
+		buf->m.planes = plane;
+	}
+
+	rc = msm_isp_prepare_v4l2_buf(buf_mgr, buf_info, buf);
+	if (rc < 0) {
+		pr_err("%s: Prepare buffer error\n", __func__);
+		kfree(plane);
+		return rc;
+	}
+	spin_lock_irqsave(&bufq->bufq_lock, flags);
+	buf_info->state = MSM_ISP_BUFFER_STATE_PREPARED;
+	spin_unlock_irqrestore(&bufq->bufq_lock, flags);
+	kfree(plane);
+	return rc;
+}
+
+static int msm_isp_buf_unprepare(struct msm_isp_buf_mgr *buf_mgr,
+	uint32_t buf_handle)
+{
+	int rc = -1, i;
+	struct msm_isp_bufq *bufq = NULL;
+	struct msm_isp_buffer *buf_info = NULL;
+	bufq = msm_isp_get_bufq(buf_mgr, buf_handle);
+	if (!bufq) {
+		pr_err("%s: Invalid bufq\n", __func__);
+		return rc;
+	}
+
+	for (i = 0; i < bufq->num_bufs; i++) {
+		buf_info = msm_isp_get_buf_ptr(buf_mgr, buf_handle, i);
+		if (buf_info->state == MSM_ISP_BUFFER_STATE_UNUSED ||
+				buf_info->state ==
+					MSM_ISP_BUFFER_STATE_INITIALIZED)
+			continue;
+
+		if (!BUF_SRC(bufq->stream_id)) {
+			if (buf_info->state == MSM_ISP_BUFFER_STATE_DEQUEUED ||
+			buf_info->state == MSM_ISP_BUFFER_STATE_DIVERTED)
+				buf_mgr->vb2_ops->put_buf(buf_info->vb2_buf,
+					bufq->session_id, bufq->stream_id);
+		}
+		msm_isp_unprepare_v4l2_buf(buf_mgr, buf_info);
+	}
+	return 0;
+}
+
+static int msm_isp_get_buf(struct msm_isp_buf_mgr *buf_mgr, uint32_t id,
+	uint32_t bufq_handle, struct msm_isp_buffer **buf_info)
+{
+	int rc = -1;
+	unsigned long flags;
+	struct msm_isp_buffer *temp_buf_info;
+	struct msm_isp_bufq *bufq = NULL;
+	struct vb2_buffer *vb2_buf = NULL;
+	bufq = msm_isp_get_bufq(buf_mgr, bufq_handle);
+	if (!bufq) {
+		pr_err("%s: Invalid bufq\n", __func__);
+		return rc;
+	}
+
+	*buf_info = NULL;
+	spin_lock_irqsave(&bufq->bufq_lock, flags);
+	if (bufq->buf_type == ISP_SHARE_BUF) {
+		list_for_each_entry(temp_buf_info,
+			&bufq->share_head, share_list) {
+			if (!temp_buf_info->buf_used[id]) {
+				temp_buf_info->buf_used[id] = 1;
+				temp_buf_info->buf_get_count++;
+				if (temp_buf_info->buf_get_count ==
+					bufq->buf_client_count)
+					list_del_init(
+					&temp_buf_info->share_list);
+				if (temp_buf_info->buf_reuse_flag) {
+					kfree(temp_buf_info);
+				} else {
+					*buf_info = temp_buf_info;
+					rc = 0;
+				}
+				spin_unlock_irqrestore(
+					&bufq->bufq_lock, flags);
+				return rc;
+			}
+		}
+	}
+
+	if (BUF_SRC(bufq->stream_id)) {
+		list_for_each_entry(temp_buf_info, &bufq->head, list) {
+			if (temp_buf_info->state ==
+					MSM_ISP_BUFFER_STATE_QUEUED) {
+				/* found one buf */
+				list_del_init(&temp_buf_info->list);
+				*buf_info = temp_buf_info;
+				break;
+			}
+		}
+	} else {
+		vb2_buf = buf_mgr->vb2_ops->get_buf(
+			bufq->session_id, bufq->stream_id);
+		if (vb2_buf) {
+			if (vb2_buf->v4l2_buf.index < bufq->num_bufs) {
+				*buf_info =
+					&bufq->bufs[vb2_buf->v4l2_buf.index];
+				(*buf_info)->vb2_buf = vb2_buf;
+			} else {
+				pr_err("%s: Incorrect buf index %d\n",
+					__func__, vb2_buf->v4l2_buf.index);
+				rc = -EINVAL;
+			}
+		}
+	}
+
+	if (!(*buf_info)) {
+		if (bufq->buf_type == ISP_SHARE_BUF) {
+			temp_buf_info = kzalloc(
+			   sizeof(struct msm_isp_buffer), GFP_ATOMIC);
+			temp_buf_info->buf_reuse_flag = 1;
+			temp_buf_info->buf_used[id] = 1;
+			temp_buf_info->buf_get_count = 1;
+			list_add_tail(&temp_buf_info->share_list,
+						  &bufq->share_head);
+		}
+	} else {
+		(*buf_info)->state = MSM_ISP_BUFFER_STATE_DEQUEUED;
+		if (bufq->buf_type == ISP_SHARE_BUF) {
+			memset((*buf_info)->buf_used, 0,
+				   sizeof(uint8_t) * bufq->buf_client_count);
+			(*buf_info)->buf_used[id] = 1;
+			(*buf_info)->buf_get_count = 1;
+			(*buf_info)->buf_put_count = 0;
+			(*buf_info)->buf_reuse_flag = 0;
+			list_add_tail(&(*buf_info)->share_list,
+						  &bufq->share_head);
+		}
+		rc = 0;
+	}
+	spin_unlock_irqrestore(&bufq->bufq_lock, flags);
+	return rc;
+}
+
+static int msm_isp_put_buf(struct msm_isp_buf_mgr *buf_mgr,
+	uint32_t bufq_handle, uint32_t buf_index)
+{
+	int rc = -1;
+	unsigned long flags;
+	struct msm_isp_bufq *bufq = NULL;
+	struct msm_isp_buffer *buf_info = NULL;
+
+	bufq = msm_isp_get_bufq(buf_mgr, bufq_handle);
+	if (!bufq) {
+		pr_err("%s: Invalid bufq\n", __func__);
+		return rc;
+	}
+
+	buf_info = msm_isp_get_buf_ptr(buf_mgr, bufq_handle, buf_index);
+	if (!buf_info) {
+		pr_err("%s: buf not found\n", __func__);
+		return rc;
+	}
+
+	spin_lock_irqsave(&bufq->bufq_lock, flags);
+	switch (buf_info->state) {
+	case MSM_ISP_BUFFER_STATE_PREPARED:
+	case MSM_ISP_BUFFER_STATE_DEQUEUED:
+	case MSM_ISP_BUFFER_STATE_DIVERTED:
+		if (BUF_SRC(bufq->stream_id))
+			list_add_tail(&buf_info->list, &bufq->head);
+		else
+			buf_mgr->vb2_ops->put_buf(buf_info->vb2_buf,
+				bufq->session_id, bufq->stream_id);
+		buf_info->state = MSM_ISP_BUFFER_STATE_QUEUED;
+		rc = 0;
+		break;
+	case MSM_ISP_BUFFER_STATE_DISPATCHED:
+		buf_info->state = MSM_ISP_BUFFER_STATE_QUEUED;
+		rc = 0;
+		break;
+	case MSM_ISP_BUFFER_STATE_QUEUED:
+		rc = 0;
+		break;
+	default:
+		pr_err("%s: incorrect state = %d",
+			__func__, buf_info->state);
+		break;
+	}
+	spin_unlock_irqrestore(&bufq->bufq_lock, flags);
+
+	return rc;
+}
+
+static int msm_isp_buf_done(struct msm_isp_buf_mgr *buf_mgr,
+	uint32_t bufq_handle, uint32_t buf_index,
+	struct timeval *tv, uint32_t frame_id, uint32_t output_format)
+{
+	int rc = -1;
+	unsigned long flags;
+	struct msm_isp_bufq *bufq = NULL;
+	struct msm_isp_buffer *buf_info = NULL;
+	enum msm_isp_buffer_state state;
+
+	bufq = msm_isp_get_bufq(buf_mgr, bufq_handle);
+	if (!bufq) {
+		pr_err("Invalid bufq\n");
+		return rc;
+	}
+
+	buf_info = msm_isp_get_buf_ptr(buf_mgr, bufq_handle, buf_index);
+	if (!buf_info) {
+		pr_err("%s: buf not found\n", __func__);
+		return rc;
+	}
+
+	spin_lock_irqsave(&bufq->bufq_lock, flags);
+	state = buf_info->state;
+	spin_unlock_irqrestore(&bufq->bufq_lock, flags);
+
+	if (state == MSM_ISP_BUFFER_STATE_DEQUEUED ||
+		state == MSM_ISP_BUFFER_STATE_DIVERTED) {
+		spin_lock_irqsave(&bufq->bufq_lock, flags);
+		if (bufq->buf_type == ISP_SHARE_BUF) {
+			buf_info->buf_put_count++;
+			if (buf_info->buf_put_count != ISP_SHARE_BUF_CLIENT) {
+				rc = buf_info->buf_put_count;
+				spin_unlock_irqrestore(&bufq->bufq_lock, flags);
+				return rc;
+			}
+		}
+		buf_info->state = MSM_ISP_BUFFER_STATE_DISPATCHED;
+		spin_unlock_irqrestore(&bufq->bufq_lock, flags);
+		if ((BUF_SRC(bufq->stream_id))) {
+			rc = msm_isp_put_buf(buf_mgr, buf_info->bufq_handle,
+						buf_info->buf_idx);
+			if (rc < 0) {
+				pr_err("%s: Buf put failed\n", __func__);
+				return rc;
+			}
+		} else {
+			buf_info->vb2_buf->v4l2_buf.timestamp = *tv;
+			buf_info->vb2_buf->v4l2_buf.sequence  = frame_id;
+			buf_info->vb2_buf->v4l2_buf.reserved = output_format;
+			buf_mgr->vb2_ops->buf_done(buf_info->vb2_buf,
+				bufq->session_id, bufq->stream_id);
+		}
+	}
+
+	return 0;
+}
+
+static int msm_isp_flush_buf(struct msm_isp_buf_mgr *buf_mgr,
+		uint32_t bufq_handle, enum msm_isp_buffer_flush_t flush_type)
+{
+	int rc = -1, i;
+	unsigned long flags;
+	struct msm_isp_bufq *bufq = NULL;
+	struct msm_isp_buffer *buf_info = NULL;
+
+	bufq = msm_isp_get_bufq(buf_mgr, bufq_handle);
+	if (!bufq) {
+		pr_err("Invalid bufq\n");
+		return rc;
+	}
+
+	for (i = 0; i < bufq->num_bufs; i++) {
+		buf_info = msm_isp_get_buf_ptr(buf_mgr, bufq_handle, i);
+		if (!buf_info) {
+			pr_err("%s: buf not found\n", __func__);
+			continue;
+		}
+
+		spin_lock_irqsave(&bufq->bufq_lock, flags);
+		if (flush_type == MSM_ISP_BUFFER_FLUSH_DIVERTED &&
+			buf_info->state == MSM_ISP_BUFFER_STATE_DIVERTED) {
+			buf_info->state = MSM_ISP_BUFFER_STATE_QUEUED;
+		} else if (flush_type == MSM_ISP_BUFFER_FLUSH_ALL &&
+			(buf_info->state == MSM_ISP_BUFFER_STATE_DEQUEUED ||
+			buf_info->state == MSM_ISP_BUFFER_STATE_DIVERTED ||
+			buf_info->state == MSM_ISP_BUFFER_STATE_DISPATCHED)) {
+			buf_info->state = MSM_ISP_BUFFER_STATE_QUEUED;
+		}
+		spin_unlock_irqrestore(&bufq->bufq_lock, flags);
+	}
+	return 0;
+}
+
+static int msm_isp_buf_divert(struct msm_isp_buf_mgr *buf_mgr,
+	uint32_t bufq_handle, uint32_t buf_index,
+	struct timeval *tv, uint32_t frame_id)
+{
+	int rc = -1;
+	unsigned long flags;
+	struct msm_isp_bufq *bufq = NULL;
+	struct msm_isp_buffer *buf_info = NULL;
+
+	bufq = msm_isp_get_bufq(buf_mgr, bufq_handle);
+	if (!bufq) {
+		pr_err("Invalid bufq\n");
+		return rc;
+	}
+
+	buf_info = msm_isp_get_buf_ptr(buf_mgr, bufq_handle, buf_index);
+	if (!buf_info) {
+		pr_err("%s: buf not found\n", __func__);
+		return rc;
+	}
+
+	spin_lock_irqsave(&bufq->bufq_lock, flags);
+	if (bufq->buf_type == ISP_SHARE_BUF) {
+		buf_info->buf_put_count++;
+		if (buf_info->buf_put_count != ISP_SHARE_BUF_CLIENT) {
+			rc = buf_info->buf_put_count;
+			spin_unlock_irqrestore(&bufq->bufq_lock, flags);
+			return rc;
+		}
+	}
+
+	if (buf_info->state == MSM_ISP_BUFFER_STATE_DEQUEUED) {
+		buf_info->state = MSM_ISP_BUFFER_STATE_DIVERTED;
+		buf_info->tv = tv;
+		buf_info->frame_id = frame_id;
+	}
+	spin_unlock_irqrestore(&bufq->bufq_lock, flags);
+
+	return 0;
+}
+
+static int msm_isp_buf_enqueue(struct msm_isp_buf_mgr *buf_mgr,
+	struct msm_isp_qbuf_info *info)
+{
+	int rc = -1, buf_state;
+	struct msm_isp_bufq *bufq = NULL;
+	struct msm_isp_buffer *buf_info = NULL;
+	buf_state = msm_isp_buf_prepare(buf_mgr, info, NULL);
+	if (buf_state < 0) {
+		pr_err("%s: Buf prepare failed\n", __func__);
+		return -EINVAL;
+	}
+
+	if (buf_state == MSM_ISP_BUFFER_STATE_DIVERTED) {
+		buf_info = msm_isp_get_buf_ptr(buf_mgr,
+						info->handle, info->buf_idx);
+		if (info->dirty_buf) {
+			rc = msm_isp_put_buf(buf_mgr,
+				info->handle, info->buf_idx);
+		} else {
+			if (BUF_SRC(bufq->stream_id))
+				pr_err("%s: Invalid native buffer state\n",
+					__func__);
+			else
+				rc = msm_isp_buf_done(buf_mgr,
+					info->handle, info->buf_idx,
+					buf_info->tv, buf_info->frame_id, 0);
+		}
+	} else {
+		bufq = msm_isp_get_bufq(buf_mgr, info->handle);
+		if (BUF_SRC(bufq->stream_id)) {
+			rc = msm_isp_put_buf(buf_mgr,
+					info->handle, info->buf_idx);
+			if (rc < 0) {
+				pr_err("%s: Buf put failed\n", __func__);
+				return rc;
+			}
+		}
+	}
+	return rc;
+}
+
+static int msm_isp_get_bufq_handle(struct msm_isp_buf_mgr *buf_mgr,
+	uint32_t session_id, uint32_t stream_id)
+{
+	int i;
+	for (i = 0; i < buf_mgr->num_buf_q; i++) {
+		if (buf_mgr->bufq[i].session_id == session_id &&
+			buf_mgr->bufq[i].stream_id == stream_id) {
+			return buf_mgr->bufq[i].bufq_handle;
+		}
+	}
+	return 0;
+}
+
+static int msm_isp_request_bufq(struct msm_isp_buf_mgr *buf_mgr,
+	struct msm_isp_buf_request *buf_request)
+{
+	int rc = -1, i;
+	struct msm_isp_bufq *bufq = NULL;
+	CDBG("%s: E\n", __func__);
+
+	if (!buf_request->num_buf) {
+		pr_err("Invalid buffer request\n");
+		return rc;
+	}
+
+	buf_request->handle = msm_isp_get_buf_handle(buf_mgr,
+		buf_request->session_id, buf_request->stream_id);
+	if (!buf_request->handle) {
+		pr_err("Invalid buffer handle\n");
+		return rc;
+	}
+
+	bufq = msm_isp_get_bufq(buf_mgr, buf_request->handle);
+	if (!bufq) {
+		pr_err("Invalid buffer queue\n");
+		return rc;
+	}
+
+	bufq->bufs = kzalloc(sizeof(struct msm_isp_buffer) *
+		buf_request->num_buf, GFP_KERNEL);
+	if (!bufq->bufs) {
+		pr_err("No free memory for buf info\n");
+		msm_isp_free_buf_handle(buf_mgr, buf_request->handle);
+		return rc;
+	}
+
+	spin_lock_init(&bufq->bufq_lock);
+	bufq->bufq_handle = buf_request->handle;
+	bufq->session_id = buf_request->session_id;
+	bufq->stream_id = buf_request->stream_id;
+	bufq->num_bufs = buf_request->num_buf;
+	bufq->buf_type = buf_request->buf_type;
+	if (bufq->buf_type == ISP_SHARE_BUF)
+		bufq->buf_client_count = ISP_SHARE_BUF_CLIENT;
+	INIT_LIST_HEAD(&bufq->head);
+	INIT_LIST_HEAD(&bufq->share_head);
+	for (i = 0; i < buf_request->num_buf; i++) {
+		bufq->bufs[i].state = MSM_ISP_BUFFER_STATE_INITIALIZED;
+		bufq->bufs[i].bufq_handle = bufq->bufq_handle;
+		bufq->bufs[i].buf_idx = i;
+	}
+
+	return 0;
+}
+
+static int msm_isp_release_bufq(struct msm_isp_buf_mgr *buf_mgr,
+	uint32_t bufq_handle)
+{
+	struct msm_isp_bufq *bufq = NULL;
+	int rc = -1;
+	bufq = msm_isp_get_bufq(buf_mgr, bufq_handle);
+	if (!bufq) {
+		pr_err("Invalid bufq release\n");
+		return rc;
+	}
+
+	msm_isp_buf_unprepare(buf_mgr, bufq_handle);
+
+	kfree(bufq->bufs);
+	msm_isp_free_buf_handle(buf_mgr, bufq_handle);
+	return 0;
+}
+
+static void msm_isp_release_all_bufq(
+	struct msm_isp_buf_mgr *buf_mgr)
+{
+	struct msm_isp_bufq *bufq = NULL;
+	int i;
+	for (i = 0; i < buf_mgr->num_buf_q; i++) {
+		bufq = &buf_mgr->bufq[i];
+		if (!bufq->bufq_handle)
+			continue;
+		msm_isp_buf_unprepare(buf_mgr, bufq->bufq_handle);
+		kfree(bufq->bufs);
+		msm_isp_free_buf_handle(buf_mgr, bufq->bufq_handle);
+	}
+}
+
+static void msm_isp_register_ctx(struct msm_isp_buf_mgr *buf_mgr,
+	struct device **iommu_ctx, int num_iommu_ctx)
+{
+	int i;
+	buf_mgr->num_iommu_ctx = num_iommu_ctx;
+	for (i = 0; i < num_iommu_ctx; i++)
+		buf_mgr->iommu_ctx[i] = iommu_ctx[i];
+}
+
+static int msm_isp_attach_ctx(struct msm_isp_buf_mgr *buf_mgr)
+{
+	int rc, i;
+	for (i = 0; i < buf_mgr->num_iommu_ctx; i++) {
+		rc = iommu_attach_device(buf_mgr->iommu_domain,
+			buf_mgr->iommu_ctx[i]);
+		if (rc) {
+			pr_err("%s: Iommu attach error\n", __func__);
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+static void msm_isp_detach_ctx(struct msm_isp_buf_mgr *buf_mgr)
+{
+	int i;
+	for (i = 0; i < buf_mgr->num_iommu_ctx; i++)
+		iommu_detach_device(buf_mgr->iommu_domain,
+			buf_mgr->iommu_ctx[i]);
+}
+
+static int msm_isp_init_isp_buf_mgr(
+	struct msm_isp_buf_mgr *buf_mgr,
+	const char *ctx_name, uint16_t num_buf_q)
+{
+	int rc = -1;
+	if (buf_mgr->open_count++)
+		return 0;
+
+	if (!num_buf_q) {
+		pr_err("Invalid buffer queue number\n");
+		return rc;
+	}
+
+	CDBG("%s: E\n", __func__);
+	msm_isp_attach_ctx(buf_mgr);
+	buf_mgr->num_buf_q = num_buf_q;
+	buf_mgr->bufq =
+		kzalloc(sizeof(struct msm_isp_bufq) * num_buf_q,
+		GFP_KERNEL);
+	if (!buf_mgr->bufq) {
+		pr_err("Bufq malloc error\n");
+		goto bufq_error;
+	}
+	buf_mgr->client = msm_ion_client_create(-1, ctx_name);
+	buf_mgr->buf_handle_cnt = 0;
+	return 0;
+bufq_error:
+	return rc;
+}
+
+static int msm_isp_deinit_isp_buf_mgr(
+	struct msm_isp_buf_mgr *buf_mgr)
+{
+	if (--buf_mgr->open_count)
+		return 0;
+	msm_isp_release_all_bufq(buf_mgr);
+	ion_client_destroy(buf_mgr->client);
+	kfree(buf_mgr->bufq);
+	buf_mgr->num_buf_q = 0;
+	msm_isp_detach_ctx(buf_mgr);
+	return 0;
+}
+
+int msm_isp_proc_buf_cmd(struct msm_isp_buf_mgr *buf_mgr,
+	unsigned int cmd, void *arg)
+{
+	switch (cmd) {
+	case VIDIOC_MSM_ISP_REQUEST_BUF: {
+		struct msm_isp_buf_request *buf_req = arg;
+		buf_mgr->ops->request_buf(buf_mgr, buf_req);
+		break;
+	}
+	case VIDIOC_MSM_ISP_ENQUEUE_BUF: {
+		struct msm_isp_qbuf_info *qbuf_info = arg;
+		buf_mgr->ops->enqueue_buf(buf_mgr, qbuf_info);
+		break;
+	}
+	case VIDIOC_MSM_ISP_RELEASE_BUF: {
+		struct msm_isp_buf_request *buf_req = arg;
+		buf_mgr->ops->release_buf(buf_mgr, buf_req->handle);
+		break;
+	}
+	}
+	return 0;
+}
+
+static struct msm_isp_buf_ops isp_buf_ops = {
+	.request_buf = msm_isp_request_bufq,
+	.enqueue_buf = msm_isp_buf_enqueue,
+	.release_buf = msm_isp_release_bufq,
+	.get_bufq_handle = msm_isp_get_bufq_handle,
+	.get_buf = msm_isp_get_buf,
+	.put_buf = msm_isp_put_buf,
+	.flush_buf = msm_isp_flush_buf,
+	.buf_done = msm_isp_buf_done,
+	.buf_divert = msm_isp_buf_divert,
+	.register_ctx = msm_isp_register_ctx,
+	.buf_mgr_init = msm_isp_init_isp_buf_mgr,
+	.buf_mgr_deinit = msm_isp_deinit_isp_buf_mgr,
+};
+
+int msm_isp_create_isp_buf_mgr(
+	struct msm_isp_buf_mgr *buf_mgr,
+	struct msm_sd_req_vb2_q *vb2_ops,
+	struct msm_iova_layout *iova_layout)
+{
+	int rc = 0;
+	if (buf_mgr->init_done)
+		return rc;
+
+	buf_mgr->iommu_domain_num = msm_register_domain(iova_layout);
+	if (buf_mgr->iommu_domain_num < 0) {
+		pr_err("%s: Invalid iommu domain number\n", __func__);
+		rc = -1;
+		goto iommu_domain_error;
+	}
+
+	buf_mgr->iommu_domain = msm_get_iommu_domain(
+		buf_mgr->iommu_domain_num);
+	if (!buf_mgr->iommu_domain) {
+		pr_err("%s: Invalid iommu domain\n", __func__);
+		rc = -1;
+		goto iommu_domain_error;
+	}
+
+	buf_mgr->ops = &isp_buf_ops;
+	buf_mgr->vb2_ops = vb2_ops;
+	buf_mgr->init_done = 1;
+	buf_mgr->open_count = 0;
+	return 0;
+iommu_domain_error:
+	return rc;
+}

kernel/drivers/media/platform/msm/camera_v2/isp/msm_buf_mgr.h

@@ -0,0 +1,149 @@
+/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _MSM_ISP_BUF_H_
+#define _MSM_ISP_BUF_H_
+
+#include <media/msmb_isp.h>
+#include <mach/iommu_domains.h>
+#include "msm_sd.h"
+
+/*Buffer source can be from userspace / HAL*/
+#define BUF_SRC(id) (id & ISP_NATIVE_BUF_BIT)
+#define ISP_SHARE_BUF_CLIENT 2
+
+struct msm_isp_buf_mgr;
+
+enum msm_isp_buffer_state {
+	MSM_ISP_BUFFER_STATE_UNUSED,         /* not used */
+	MSM_ISP_BUFFER_STATE_INITIALIZED,    /* REQBUF done */
+	MSM_ISP_BUFFER_STATE_PREPARED,       /* BUF mapped */
+	MSM_ISP_BUFFER_STATE_QUEUED,         /* buf queued */
+	MSM_ISP_BUFFER_STATE_DEQUEUED,       /* in use in VFE */
+	MSM_ISP_BUFFER_STATE_DIVERTED,       /* Sent to other hardware*/
+	MSM_ISP_BUFFER_STATE_DISPATCHED,     /* Sent to HAL*/
+};
+
+enum msm_isp_buffer_flush_t {
+	MSM_ISP_BUFFER_FLUSH_DIVERTED,
+	MSM_ISP_BUFFER_FLUSH_ALL,
+};
+
+struct msm_isp_buffer_mapped_info {
+	unsigned long len;
+	unsigned long paddr;
+	struct ion_handle *handle;
+};
+
+struct msm_isp_buffer {
+	/*Common Data structure*/
+	int num_planes;
+	struct msm_isp_buffer_mapped_info mapped_info[VIDEO_MAX_PLANES];
+	int buf_idx;
+	uint32_t bufq_handle;
+	uint32_t frame_id;
+	struct timeval *tv;
+
+	/*Native buffer*/
+	struct list_head list;
+	enum msm_isp_buffer_state state;
+
+	/*Vb2 buffer data*/
+	struct vb2_buffer *vb2_buf;
+
+	/*Share buffer cache state*/
+	struct list_head share_list;
+	uint8_t buf_used[ISP_SHARE_BUF_CLIENT];
+	uint8_t buf_get_count;
+	uint8_t buf_put_count;
+	uint8_t buf_reuse_flag;
+};
+
+struct msm_isp_bufq {
+	uint32_t session_id;
+	uint32_t stream_id;
+	uint32_t num_bufs;
+	uint32_t bufq_handle;
+	enum msm_isp_buf_type buf_type;
+	struct msm_isp_buffer *bufs;
+	spinlock_t bufq_lock;
+
+	/*Native buffer queue*/
+	struct list_head head;
+	/*Share buffer cache queue*/
+	struct list_head share_head;
+	uint8_t buf_client_count;
+};
+
+struct msm_isp_buf_ops {
+	int (*request_buf) (struct msm_isp_buf_mgr *buf_mgr,
+		struct msm_isp_buf_request *buf_request);
+
+	int (*enqueue_buf) (struct msm_isp_buf_mgr *buf_mgr,
+		struct msm_isp_qbuf_info *info);
+
+	int (*release_buf) (struct msm_isp_buf_mgr *buf_mgr,
+		uint32_t bufq_handle);
+
+	int (*get_bufq_handle) (struct msm_isp_buf_mgr *buf_mgr,
+		uint32_t session_id, uint32_t stream_id);
+
+	int (*get_buf) (struct msm_isp_buf_mgr *buf_mgr, uint32_t id,
+		uint32_t bufq_handle, struct msm_isp_buffer **buf_info);
+
+	int (*put_buf) (struct msm_isp_buf_mgr *buf_mgr,
+		uint32_t bufq_handle, uint32_t buf_index);
+
+	int (*flush_buf) (struct msm_isp_buf_mgr *buf_mgr,
+		uint32_t bufq_handle, enum msm_isp_buffer_flush_t flush_type);
+
+	int (*buf_done) (struct msm_isp_buf_mgr *buf_mgr,
+		uint32_t bufq_handle, uint32_t buf_index,
+		struct timeval *tv, uint32_t frame_id, uint32_t output_format);
+	int (*buf_divert) (struct msm_isp_buf_mgr *buf_mgr,
+		uint32_t bufq_handle, uint32_t buf_index,
+		struct timeval *tv, uint32_t frame_id);
+	void (*register_ctx) (struct msm_isp_buf_mgr *buf_mgr,
+		struct device **iommu_ctx, int num_iommu_ctx);
+	int (*buf_mgr_init) (struct msm_isp_buf_mgr *buf_mgr,
+		const char *ctx_name, uint16_t num_buf_q);
+	int (*buf_mgr_deinit) (struct msm_isp_buf_mgr *buf_mgr);
+};
+
+struct msm_isp_buf_mgr {
+	int init_done;
+	uint32_t open_count;
+	spinlock_t lock;
+	uint16_t num_buf_q;
+	struct msm_isp_bufq *bufq;
+
+	struct ion_client *client;
+	struct msm_isp_buf_ops *ops;
+	uint32_t buf_handle_cnt;
+
+	struct msm_sd_req_vb2_q *vb2_ops;
+
+	/*IOMMU specific*/
+	int iommu_domain_num;
+	struct iommu_domain *iommu_domain;
+
+	int num_iommu_ctx;
+	struct device *iommu_ctx[2];
+};
+
+int msm_isp_create_isp_buf_mgr(struct msm_isp_buf_mgr *buf_mgr,
+	struct msm_sd_req_vb2_q *vb2_ops, struct msm_iova_layout *iova_layout);
+
+int msm_isp_proc_buf_cmd(struct msm_isp_buf_mgr *buf_mgr,
+	unsigned int cmd, void *arg);
+
+#endif /* _MSM_ISP_BUF_H_ */

kernel/drivers/media/platform/msm/camera_v2/isp/msm_isp.c

@@ -0,0 +1,176 @@
+/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/proc_fs.h>
+#include <linux/debugfs.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <mach/board.h>
+#include <mach/vreg.h>
+#include <mach/iommu.h>
+
+#include "msm_isp.h"
+#include "msm_isp_util.h"
+#include "msm_isp_axi_util.h"
+#include "msm_isp_stats_util.h"
+#include "msm_sd.h"
+#include "msm_isp40.h"
+#include "msm_isp32.h"
+
+static struct msm_sd_req_vb2_q vfe_vb2_ops;
+
+static const struct of_device_id msm_vfe_dt_match[] = {
+	{
+		.compatible = "qcom,vfe40",
+		.data = &vfe40_hw_info,
+	},
+	{
+		.compatible = "qcom,vfe32",
+		.data = &vfe32_hw_info,
+	},
+	{}
+};
+
+MODULE_DEVICE_TABLE(of, msm_vfe_dt_match);
+
+static const struct platform_device_id msm_vfe_dev_id[] = {
+	{"msm_vfe32", (kernel_ulong_t) &vfe32_hw_info},
+	{}
+};
+
+static struct msm_isp_buf_mgr vfe_buf_mgr;
+
+static int __devinit vfe_probe(struct platform_device *pdev)
+{
+	struct vfe_device *vfe_dev;
+	/*struct msm_cam_subdev_info sd_info;*/
+	const struct of_device_id *match_dev;
+	int rc = 0;
+
+	struct msm_iova_partition vfe_partition = {
+		.start = SZ_128K,
+		.size = SZ_2G - SZ_128K,
+	};
+	struct msm_iova_layout vfe_layout = {
+		.partitions = &vfe_partition,
+		.npartitions = 1,
+		.client_name = "vfe",
+		.domain_flags = 0,
+	};
+
+	vfe_dev = kzalloc(sizeof(struct vfe_device), GFP_KERNEL);
+	if (!vfe_dev) {
+		pr_err("%s: no enough memory\n", __func__);
+		return -ENOMEM;
+	}
+
+	if (pdev->dev.of_node) {
+		of_property_read_u32((&pdev->dev)->of_node,
+			"cell-index", &pdev->id);
+		match_dev = of_match_device(msm_vfe_dt_match, &pdev->dev);
+		vfe_dev->hw_info =
+			(struct msm_vfe_hardware_info *) match_dev->data;
+	} else {
+		vfe_dev->hw_info = (struct msm_vfe_hardware_info *)
+			platform_get_device_id(pdev)->driver_data;
+	}
+
+	if (!vfe_dev->hw_info) {
+		pr_err("%s: No vfe hardware info\n", __func__);
+		return -EINVAL;
+	}
+	ISP_DBG("%s: device id = %d\n", __func__, pdev->id);
+
+	vfe_dev->pdev = pdev;
+	rc = vfe_dev->hw_info->vfe_ops.core_ops.get_platform_data(vfe_dev);
+	if (rc < 0) {
+		pr_err("%s: failed to get platform resources\n", __func__);
+		kfree(vfe_dev);
+		return -ENOMEM;
+	}
+
+	INIT_LIST_HEAD(&vfe_dev->tasklet_q);
+	tasklet_init(&vfe_dev->vfe_tasklet,
+		msm_isp_do_tasklet, (unsigned long)vfe_dev);
+
+	v4l2_subdev_init(&vfe_dev->subdev.sd, vfe_dev->hw_info->subdev_ops);
+	vfe_dev->subdev.sd.internal_ops =
+		vfe_dev->hw_info->subdev_internal_ops;
+	snprintf(vfe_dev->subdev.sd.name,
+		ARRAY_SIZE(vfe_dev->subdev.sd.name),
+		"vfe");
+	vfe_dev->subdev.sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	vfe_dev->subdev.sd.flags |= V4L2_SUBDEV_FL_HAS_EVENTS;
+	v4l2_set_subdevdata(&vfe_dev->subdev.sd, vfe_dev);
+	platform_set_drvdata(pdev, &vfe_dev->subdev.sd);
+	mutex_init(&vfe_dev->realtime_mutex);
+	mutex_init(&vfe_dev->core_mutex);
+	spin_lock_init(&vfe_dev->tasklet_lock);
+	spin_lock_init(&vfe_dev->shared_data_lock);
+	media_entity_init(&vfe_dev->subdev.sd.entity, 0, NULL, 0);
+	vfe_dev->subdev.sd.entity.type = MEDIA_ENT_T_V4L2_SUBDEV;
+	vfe_dev->subdev.sd.entity.group_id = MSM_CAMERA_SUBDEV_VFE;
+	vfe_dev->subdev.sd.entity.name = pdev->name;
+	vfe_dev->subdev.close_seq = MSM_SD_CLOSE_1ST_CATEGORY | 0x2;
+	rc = msm_sd_register(&vfe_dev->subdev);
+	if (rc != 0) {
+		pr_err("%s: msm_sd_register error = %d\n", __func__, rc);
+		kfree(vfe_dev);
+		goto end;
+	}
+
+	vfe_dev->buf_mgr = &vfe_buf_mgr;
+	v4l2_subdev_notify(&vfe_dev->subdev.sd,
+		MSM_SD_NOTIFY_REQ_CB, &vfe_vb2_ops);
+	rc = msm_isp_create_isp_buf_mgr(vfe_dev->buf_mgr,
+		&vfe_vb2_ops, &vfe_layout);
+	if (rc < 0) {
+		pr_err("%s: Unable to create buffer manager\n", __func__);
+		kfree(vfe_dev);
+		return -EINVAL;
+	}
+	vfe_dev->buf_mgr->ops->register_ctx(vfe_dev->buf_mgr,
+		&vfe_dev->iommu_ctx[0], vfe_dev->hw_info->num_iommu_ctx);
+	vfe_dev->vfe_open_cnt = 0;
+end:
+	return rc;
+}
+
+static struct platform_driver vfe_driver = {
+	.probe = vfe_probe,
+	.driver = {
+		.name = "msm_vfe",
+		.owner = THIS_MODULE,
+		.of_match_table = msm_vfe_dt_match,
+	},
+	.id_table = msm_vfe_dev_id,
+};
+
+static int __init msm_vfe_init_module(void)
+{
+	return platform_driver_register(&vfe_driver);
+}
+
+static void __exit msm_vfe_exit_module(void)
+{
+	platform_driver_unregister(&vfe_driver);
+}
+
+module_init(msm_vfe_init_module);
+module_exit(msm_vfe_exit_module);
+MODULE_DESCRIPTION("MSM VFE driver");
+MODULE_LICENSE("GPL v2");

kernel/drivers/media/platform/msm/camera_v2/isp/msm_isp.h

@@ -0,0 +1,426 @@
+/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MSM_VFE_H__
+#define __MSM_VFE_H__
+
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/completion.h>
+#include <linux/io.h>
+#include <linux/list.h>
+#include <linux/delay.h>
+#include <media/v4l2-subdev.h>
+#include <media/msmb_isp.h>
+#include <mach/msm_bus.h>
+#include <mach/msm_bus_board.h>
+
+#include "msm_buf_mgr.h"
+
+#define MAX_IOMMU_CTX 2
+#define MAX_NUM_WM 7
+#define MAX_NUM_RDI 3
+#define MAX_NUM_RDI_MASTER 3
+#define MAX_NUM_COMPOSITE_MASK 4
+#define MAX_NUM_STATS_COMP_MASK 2
+#define MAX_INIT_FRAME_DROP 31
+#define ISP_Q2 (1 << 2)
+
+#define VFE_PING_FLAG 0xFFFFFFFF
+#define VFE_PONG_FLAG 0x0
+
+#define VFE_MAX_CFG_TIMEOUT 3000
+
+struct vfe_device;
+struct msm_vfe_axi_stream;
+struct msm_vfe_stats_stream;
+
+struct vfe_subscribe_info {
+	struct v4l2_fh *vfh;
+	uint32_t active;
+};
+
+enum msm_isp_camif_update_state {
+	NO_UPDATE,
+	ENABLE_CAMIF,
+	DISABLE_CAMIF,
+	DISABLE_CAMIF_IMMEDIATELY
+};
+
+struct msm_isp_timestamp {
+	/*Monotonic clock for v4l2 buffer*/
+	struct timeval buf_time;
+	/*Wall clock for userspace event*/
+	struct timeval event_time;
+};
+
+struct msm_vfe_irq_ops {
+	void (*read_irq_status) (struct vfe_device *vfe_dev,
+		uint32_t *irq_status0, uint32_t *irq_status1);
+	void (*process_reg_update) (struct vfe_device *vfe_dev,
+		uint32_t irq_status0, uint32_t irq_status1,
+		struct msm_isp_timestamp *ts);
+	void (*process_reset_irq) (struct vfe_device *vfe_dev,
+		uint32_t irq_status0, uint32_t irq_status1);
+	void (*process_halt_irq) (struct vfe_device *vfe_dev,
+		uint32_t irq_status0, uint32_t irq_status1);
+	void (*process_camif_irq) (struct vfe_device *vfe_dev,
+		uint32_t irq_status0, uint32_t irq_status1,
+		struct msm_isp_timestamp *ts);
+	void (*process_axi_irq) (struct vfe_device *vfe_dev,
+		uint32_t irq_status0, uint32_t irq_status1,
+		struct msm_isp_timestamp *ts);
+	void (*process_stats_irq) (struct vfe_device *vfe_dev,
+		uint32_t irq_status0, uint32_t irq_status1,
+		struct msm_isp_timestamp *ts);
+};
+
+struct msm_vfe_axi_ops {
+	void (*reload_wm) (struct vfe_device *vfe_dev,
+		uint32_t reload_mask);
+	void (*enable_wm) (struct vfe_device *vfe_dev,
+		uint8_t wm_idx, uint8_t enable);
+	void (*cfg_io_format) (struct vfe_device *vfe_dev,
+		struct msm_vfe_axi_stream *stream_info);
+	void (*cfg_framedrop) (struct vfe_device *vfe_dev,
+		struct msm_vfe_axi_stream *stream_info);
+	void (*clear_framedrop) (struct vfe_device *vfe_dev,
+		struct msm_vfe_axi_stream *stream_info);
+	void (*cfg_comp_mask) (struct vfe_device *vfe_dev,
+		struct msm_vfe_axi_stream *stream_info);
+	void (*clear_comp_mask) (struct vfe_device *vfe_dev,
+		struct msm_vfe_axi_stream *stream_info);
+	void (*cfg_wm_irq_mask) (struct vfe_device *vfe_dev,
+		struct msm_vfe_axi_stream *stream_info);
+	void (*clear_wm_irq_mask) (struct vfe_device *vfe_dev,
+		struct msm_vfe_axi_stream *stream_info);
+
+	void (*cfg_wm_reg) (struct vfe_device *vfe_dev,
+		struct msm_vfe_axi_stream *stream_info,
+		uint8_t plane_idx);
+	void (*clear_wm_reg) (struct vfe_device *vfe_dev,
+		struct msm_vfe_axi_stream *stream_info, uint8_t plane_idx);
+
+	void (*cfg_wm_xbar_reg) (struct vfe_device *vfe_dev,
+		struct msm_vfe_axi_stream *stream_info,
+		uint8_t plane_idx);
+	void (*clear_wm_xbar_reg) (struct vfe_device *vfe_dev,
+		struct msm_vfe_axi_stream *stream_info, uint8_t plane_idx);
+
+	void (*cfg_ub) (struct vfe_device *vfe_dev);
+
+	void (*update_ping_pong_addr) (struct vfe_device *vfe_dev,
+		uint8_t wm_idx, uint32_t pingpong_status, unsigned long paddr);
+
+	uint32_t (*get_wm_mask) (uint32_t irq_status0, uint32_t irq_status1);
+	uint32_t (*get_comp_mask) (uint32_t irq_status0, uint32_t irq_status1);
+	uint32_t (*get_pingpong_status) (struct vfe_device *vfe_dev);
+	long (*halt) (struct vfe_device *vfe_dev);
+};
+
+struct msm_vfe_core_ops {
+	void (*reg_update) (struct vfe_device *vfe_dev);
+	long (*reset_hw) (struct vfe_device *vfe_dev);
+	int (*init_hw) (struct vfe_device *vfe_dev);
+	void (*init_hw_reg) (struct vfe_device *vfe_dev);
+	void (*release_hw) (struct vfe_device *vfe_dev);
+	void (*cfg_camif) (struct vfe_device *vfe_dev,
+		struct msm_vfe_pix_cfg *pix_cfg);
+	void (*update_camif_state) (struct vfe_device *vfe_dev,
+		enum msm_isp_camif_update_state update_state);
+	void (*cfg_rdi_reg) (struct vfe_device *vfe_dev,
+		struct msm_vfe_rdi_cfg *rdi_cfg,
+		enum msm_vfe_input_src input_src);
+	int (*get_platform_data) (struct vfe_device *vfe_dev);
+	void (*get_error_mask) (uint32_t *error_mask0, uint32_t *error_mask1);
+	void (*process_error_status) (struct vfe_device *vfe_dev);
+};
+struct msm_vfe_stats_ops {
+	int (*get_stats_idx) (enum msm_isp_stats_type stats_type);
+	void (*cfg_framedrop) (struct vfe_device *vfe_dev,
+		struct msm_vfe_stats_stream *stream_info);
+	void (*clear_framedrop) (struct vfe_device *vfe_dev,
+		struct msm_vfe_stats_stream *stream_info);
+	void (*cfg_comp_mask) (struct vfe_device *vfe_dev,
+		uint32_t stats_mask, uint8_t enable);
+	void (*cfg_wm_irq_mask) (struct vfe_device *vfe_dev,
+		struct msm_vfe_stats_stream *stream_info);
+	void (*clear_wm_irq_mask) (struct vfe_device *vfe_dev,
+		struct msm_vfe_stats_stream *stream_info);
+
+	void (*cfg_wm_reg) (struct vfe_device *vfe_dev,
+		struct msm_vfe_stats_stream *stream_info);
+	void (*clear_wm_reg) (struct vfe_device *vfe_dev,
+		struct msm_vfe_stats_stream *stream_info);
+
+	void (*cfg_ub) (struct vfe_device *vfe_dev);
+
+	void (*enable_module) (struct vfe_device *vfe_dev,
+		uint32_t stats_mask, uint8_t enable);
+
+	void (*update_ping_pong_addr) (struct vfe_device *vfe_dev,
+		struct msm_vfe_stats_stream *stream_info,
+		uint32_t pingpong_status, unsigned long paddr);
+
+	uint32_t (*get_frame_id) (struct vfe_device *vfe_dev);
+	uint32_t (*get_wm_mask) (uint32_t irq_status0, uint32_t irq_status1);
+	uint32_t (*get_comp_mask) (uint32_t irq_status0, uint32_t irq_status1);
+	uint32_t (*get_pingpong_status) (struct vfe_device *vfe_dev);
+};
+
+struct msm_vfe_ops {
+	struct msm_vfe_irq_ops irq_ops;
+	struct msm_vfe_axi_ops axi_ops;
+	struct msm_vfe_core_ops core_ops;
+	struct msm_vfe_stats_ops stats_ops;
+};
+
+struct msm_vfe_hardware_info {
+	int num_iommu_ctx;
+	int vfe_clk_idx;
+	struct msm_vfe_ops vfe_ops;
+	struct msm_vfe_axi_hardware_info *axi_hw_info;
+	struct msm_vfe_stats_hardware_info *stats_hw_info;
+	struct v4l2_subdev_internal_ops *subdev_internal_ops;
+	struct v4l2_subdev_ops *subdev_ops;
+	uint32_t dmi_reg_offset;
+};
+
+struct msm_vfe_axi_hardware_info {
+	uint8_t num_wm;
+	uint8_t num_rdi;
+	uint8_t num_rdi_master;
+	uint8_t num_comp_mask;
+	uint32_t min_wm_ub;
+};
+
+enum msm_vfe_axi_state {
+	AVALIABLE,
+	INACTIVE,
+	ACTIVE,
+	PAUSED,
+	START_PENDING,
+	STOP_PENDING,
+	PAUSE_PENDING,
+	RESUME_PENDING,
+	STARTING,
+	STOPPING,
+	PAUSING,
+	RESUMING,
+};
+
+enum msm_vfe_axi_cfg_update_state {
+	NO_AXI_CFG_UPDATE,
+	APPLYING_UPDATE_RESUME,
+	UPDATE_REQUESTED,
+};
+
+#define VFE_NO_DROP            0xFFFFFFFF
+#define VFE_DROP_EVERY_2FRAME  0x55555555
+#define VFE_DROP_EVERY_4FRAME  0x11111111
+#define VFE_DROP_EVERY_8FRAME  0x01010101
+#define VFE_DROP_EVERY_16FRAME 0x00010001
+#define VFE_DROP_EVERY_32FRAME 0x00000001
+
+enum msm_vfe_axi_stream_type {
+	CONTINUOUS_STREAM,
+	BURST_STREAM,
+};
+
+struct msm_vfe_axi_stream {
+	uint32_t frame_id;
+	enum msm_vfe_axi_state state;
+	enum msm_vfe_axi_stream_src stream_src;
+	uint8_t num_planes;
+	uint8_t wm[MAX_PLANES_PER_STREAM];
+	uint32_t output_format;/*Planar/RAW/Misc*/
+	struct msm_vfe_axi_plane_cfg plane_cfg[MAX_PLANES_PER_STREAM];
+	uint8_t comp_mask_index;
+	struct msm_isp_buffer *buf[2];
+	uint32_t session_id;
+	uint32_t stream_id;
+	uint32_t bufq_handle;
+	uint32_t stream_handle;
+	uint8_t buf_divert;
+	enum msm_vfe_axi_stream_type stream_type;
+
+	uint32_t frame_based;
+	uint32_t framedrop_period;
+	uint32_t framedrop_pattern;
+	uint32_t num_burst_capture;/*number of frame to capture*/
+	uint32_t init_frame_drop;
+	uint32_t burst_frame_count;/*number of sof before burst stop*/
+	uint8_t framedrop_update;
+	spinlock_t lock;
+
+	/*Bandwidth calculation info*/
+	uint32_t max_width;
+	/*Based on format plane size in Q2. e.g NV12 = 1.5*/
+	uint32_t format_factor;
+	uint32_t bandwidth;
+
+	/*Run time update variables*/
+	uint32_t runtime_init_frame_drop;
+	uint32_t runtime_burst_frame_count;/*number of sof before burst stop*/
+	uint32_t runtime_num_burst_capture;
+	uint8_t runtime_framedrop_update;
+	uint32_t runtime_output_format;
+};
+
+struct msm_vfe_axi_composite_info {
+	uint32_t stream_handle;
+	uint32_t stream_composite_mask;
+};
+
+struct msm_vfe_src_info {
+	unsigned long frame_id;
+	uint8_t active;
+	uint8_t pix_stream_count;
+	uint8_t raw_stream_count;
+	enum msm_vfe_inputmux input_mux;
+	uint32_t width;
+	long pixel_clock;
+};
+
+enum msm_wm_ub_cfg_type {
+	MSM_WM_UB_CFG_DEFAULT,
+	MSM_WM_UB_EQUAL_SLICING,
+	MSM_WM_UB_CFG_MAX_NUM
+};
+
+struct msm_vfe_axi_shared_data {
+	struct msm_vfe_axi_hardware_info *hw_info;
+	struct msm_vfe_axi_stream stream_info[MAX_NUM_STREAM];
+	uint32_t free_wm[MAX_NUM_WM];
+	uint32_t wm_image_size[MAX_NUM_WM];
+	enum msm_wm_ub_cfg_type wm_ub_cfg_policy;
+	uint8_t num_used_wm;
+	uint8_t num_active_stream;
+	struct msm_vfe_axi_composite_info
+		composite_info[MAX_NUM_COMPOSITE_MASK];
+	uint8_t num_used_composite_mask;
+	uint32_t stream_update;
+	atomic_t axi_cfg_update;
+	enum msm_isp_camif_update_state pipeline_update;
+	struct msm_vfe_src_info src_info[VFE_SRC_MAX];
+	uint16_t stream_handle_cnt;
+	unsigned long event_mask;
+};
+
+struct msm_vfe_stats_hardware_info {
+	uint32_t stats_capability_mask;
+	uint32_t stats_ping_pong_offset;
+	uint8_t num_stats_type;
+	uint8_t num_stats_comp_mask;
+};
+
+enum msm_vfe_stats_state {
+	STATS_AVALIABLE,
+	STATS_INACTIVE,
+	STATS_ACTIVE,
+	STATS_START_PENDING,
+	STATS_STOP_PENDING,
+	STATS_STARTING,
+	STATS_STOPPING,
+};
+
+struct msm_vfe_stats_stream {
+	uint32_t session_id;
+	uint32_t stream_id;
+	uint32_t stream_handle;
+	uint32_t composite_flag;
+	enum msm_isp_stats_type stats_type;
+	enum msm_vfe_stats_state state;
+	uint32_t framedrop_pattern;
+	uint32_t framedrop_period;
+	uint32_t irq_subsample_pattern;
+
+	uint32_t buffer_offset;
+	struct msm_isp_buffer *buf[2];
+	uint32_t bufq_handle;
+};
+
+struct msm_vfe_stats_shared_data {
+	struct msm_vfe_stats_stream stream_info[MSM_ISP_STATS_MAX];
+	uint8_t num_active_stream;
+	atomic_t stats_comp_mask;
+	uint16_t stream_handle_cnt;
+	atomic_t stats_update;
+};
+
+struct msm_vfe_tasklet_queue_cmd {
+	struct list_head list;
+	uint32_t vfeInterruptStatus0;
+	uint32_t vfeInterruptStatus1;
+	struct msm_isp_timestamp ts;
+	uint8_t cmd_used;
+};
+
+#define MSM_VFE_TASKLETQ_SIZE 200
+
+struct msm_vfe_error_info {
+	uint32_t error_mask0;
+	uint32_t error_mask1;
+	uint32_t violation_status;
+	uint32_t camif_status;
+	uint32_t stream_framedrop_count[MAX_NUM_STREAM];
+	uint32_t stats_framedrop_count[MSM_ISP_STATS_MAX];
+	uint32_t info_dump_frame_count;
+	uint32_t error_count;
+};
+
+struct vfe_device {
+	struct platform_device *pdev;
+	struct msm_sd_subdev subdev;
+	struct resource *vfe_irq;
+	struct resource *vfe_mem;
+	struct resource *vfe_vbif_mem;
+	struct resource *vfe_io;
+	struct resource *vfe_vbif_io;
+	void __iomem *vfe_base;
+	void __iomem *vfe_vbif_base;
+
+	struct device *iommu_ctx[MAX_IOMMU_CTX];
+
+	struct regulator *fs_vfe;
+	struct clk *vfe_clk[7];
+
+	uint32_t bus_perf_client;
+
+	struct completion reset_complete;
+	struct completion halt_complete;
+	struct completion stream_config_complete;
+	struct completion stats_config_complete;
+	struct mutex realtime_mutex;
+	struct mutex core_mutex;
+
+	atomic_t irq_cnt;
+	uint8_t taskletq_idx;
+	spinlock_t  tasklet_lock;
+	spinlock_t  shared_data_lock;
+	struct list_head tasklet_q;
+	struct tasklet_struct vfe_tasklet;
+	struct msm_vfe_tasklet_queue_cmd
+		tasklet_queue_cmd[MSM_VFE_TASKLETQ_SIZE];
+
+	uint32_t vfe_hw_version;
+	struct msm_vfe_hardware_info *hw_info;
+	struct msm_vfe_axi_shared_data axi_data;
+	struct msm_vfe_stats_shared_data stats_data;
+	struct msm_vfe_error_info error_info;
+	struct msm_isp_buf_mgr *buf_mgr;
+	int dump_reg;
+	int vfe_clk_idx;
+	uint32_t vfe_open_cnt;
+};
+
+#endif

kernel/drivers/media/platform/msm/camera_v2/isp/msm_isp32.h

@@ -0,0 +1,17 @@
+/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MSM_ISP32_H__
+#define __MSM_ISP32_H__
+
+extern struct msm_vfe_hardware_info vfe32_hw_info;
+#endif /* __MSM_ISP32_H__ */

kernel/drivers/media/platform/msm/camera_v2/isp/msm_isp40.h

@@ -0,0 +1,17 @@
+/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MSM_ISP40_H__
+#define __MSM_ISP40_H__
+
+extern struct msm_vfe_hardware_info vfe40_hw_info;
+#endif /* __MSM_ISP40_H__ */

kernel/drivers/media/platform/msm/camera_v2/isp/msm_isp_axi_util.h

@@ -0,0 +1,54 @@
+/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#ifndef __MSM_ISP_AXI_UTIL_H__
+#define __MSM_ISP_AXI_UTIL_H__
+
+#include "msm_isp.h"
+
+int msm_isp_axi_create_stream(
+	struct msm_vfe_axi_shared_data *axi_data,
+	struct msm_vfe_axi_stream_request_cmd *stream_cfg_cmd);
+
+void msm_isp_axi_destroy_stream(
+	struct msm_vfe_axi_shared_data *axi_data, int stream_idx);
+
+int msm_isp_validate_axi_request(
+	struct msm_vfe_axi_shared_data *axi_data,
+	struct msm_vfe_axi_stream_request_cmd *stream_cfg_cmd);
+
+void msm_isp_axi_reserve_wm(
+	struct msm_vfe_axi_shared_data *axi_data,
+	struct msm_vfe_axi_stream *stream_info);
+
+void msm_isp_axi_reserve_comp_mask(
+	struct msm_vfe_axi_shared_data *axi_data,
+	struct msm_vfe_axi_stream *stream_info);
+
+int msm_isp_axi_check_stream_state(
+	struct vfe_device *vfe_dev,
+	struct msm_vfe_axi_stream_cfg_cmd *stream_cfg_cmd);
+
+int msm_isp_request_axi_stream(struct vfe_device *vfe_dev, void *arg);
+int msm_isp_cfg_axi_stream(struct vfe_device *vfe_dev, void *arg);
+int msm_isp_release_axi_stream(struct vfe_device *vfe_dev, void *arg);
+int msm_isp_update_axi_stream(struct vfe_device *vfe_dev, void *arg);
+void msm_isp_axi_cfg_update(struct vfe_device *vfe_dev);
+
+void msm_isp_axi_stream_update(struct vfe_device *vfe_dev);
+
+void msm_isp_update_framedrop_reg(struct vfe_device *vfe_dev);
+void msm_isp_sof_notify(struct vfe_device *vfe_dev,
+	enum msm_vfe_input_src frame_src, struct msm_isp_timestamp *ts);
+void msm_isp_process_axi_irq(struct vfe_device *vfe_dev,
+	uint32_t irq_status0, uint32_t irq_status1,
+	struct msm_isp_timestamp *ts);
+#endif /* __MSM_ISP_AXI_UTIL_H__ */

kernel/drivers/media/platform/msm/camera_v2/isp/msm_isp_stats_util.c

@@ -0,0 +1,458 @@
+/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/io.h>
+#include <linux/atomic.h>
+#include <media/v4l2-subdev.h>
+#include "msm_isp_util.h"
+#include "msm_isp_stats_util.h"
+
+static int msm_isp_stats_cfg_ping_pong_address(struct vfe_device *vfe_dev,
+	struct msm_vfe_stats_stream *stream_info, uint32_t pingpong_status,
+	struct msm_isp_buffer **done_buf)
+{
+	int rc = -1;
+	struct msm_isp_buffer *buf;
+	uint32_t pingpong_bit = 0;
+	uint32_t bufq_handle = stream_info->bufq_handle;
+	uint32_t stats_pingpong_offset =
+		STATS_IDX(stream_info->stream_handle) +
+		vfe_dev->hw_info->stats_hw_info->stats_ping_pong_offset;
+
+	pingpong_bit = (~(pingpong_status >> stats_pingpong_offset) & 0x1);
+	rc = vfe_dev->buf_mgr->ops->get_buf(vfe_dev->buf_mgr,
+			vfe_dev->pdev->id, bufq_handle, &buf);
+	if (rc < 0) {
+		vfe_dev->error_info.stats_framedrop_count[
+			STATS_IDX(stream_info->stream_handle)]++;
+		return rc;
+	}
+
+	if (buf->num_planes != 1) {
+		pr_err("%s: Invalid buffer\n", __func__);
+		rc = -EINVAL;
+		goto buf_error;
+	}
+
+	vfe_dev->hw_info->vfe_ops.stats_ops.update_ping_pong_addr(
+		vfe_dev, stream_info,
+		pingpong_status, buf->mapped_info[0].paddr +
+		stream_info->buffer_offset);
+
+	if (stream_info->buf[pingpong_bit] && done_buf)
+		*done_buf = stream_info->buf[pingpong_bit];
+
+	stream_info->buf[pingpong_bit] = buf;
+	return 0;
+buf_error:
+	vfe_dev->buf_mgr->ops->put_buf(vfe_dev->buf_mgr,
+		buf->bufq_handle, buf->buf_idx);
+	return rc;
+}
+
+void msm_isp_process_stats_irq(struct vfe_device *vfe_dev,
+	uint32_t irq_status0, uint32_t irq_status1,
+	struct msm_isp_timestamp *ts)
+{
+	int i, rc;
+	struct msm_isp_event_data buf_event;
+	struct msm_isp_stats_event *stats_event = &buf_event.u.stats;
+	struct msm_isp_buffer *done_buf;
+	struct msm_vfe_stats_stream *stream_info = NULL;
+	uint32_t pingpong_status;
+	uint32_t comp_stats_type_mask = 0;
+	uint32_t stats_comp_mask = 0, stats_irq_mask = 0;
+	stats_comp_mask = vfe_dev->hw_info->vfe_ops.stats_ops.
+		get_comp_mask(irq_status0, irq_status1);
+	stats_irq_mask = vfe_dev->hw_info->vfe_ops.stats_ops.
+		get_wm_mask(irq_status0, irq_status1);
+	if (!(stats_comp_mask || stats_irq_mask))
+		return;
+	ISP_DBG("%s: status: 0x%x\n", __func__, irq_status0);
+
+	if (!stats_comp_mask)
+		stats_irq_mask &=
+			~atomic_read(&vfe_dev->stats_data.stats_comp_mask);
+	else
+		stats_irq_mask |=
+			atomic_read(&vfe_dev->stats_data.stats_comp_mask);
+
+	memset(&buf_event, 0, sizeof(struct msm_isp_event_data));
+	buf_event.timestamp = ts->event_time;
+	buf_event.frame_id =
+		vfe_dev->axi_data.src_info[VFE_PIX_0].frame_id;
+	pingpong_status = vfe_dev->hw_info->
+		vfe_ops.stats_ops.get_pingpong_status(vfe_dev);
+
+	for (i = 0; i < vfe_dev->hw_info->stats_hw_info->num_stats_type; i++) {
+		if (!(stats_irq_mask & (1 << i)))
+			continue;
+		stream_info = &vfe_dev->stats_data.stream_info[i];
+		done_buf = NULL;
+		msm_isp_stats_cfg_ping_pong_address(vfe_dev,
+			stream_info, pingpong_status, &done_buf);
+		if (done_buf) {
+			rc = vfe_dev->buf_mgr->ops->buf_divert(vfe_dev->buf_mgr,
+				done_buf->bufq_handle, done_buf->buf_idx,
+				&ts->buf_time, vfe_dev->axi_data.
+				src_info[VFE_PIX_0].frame_id);
+			if (rc != 0)
+				continue;
+
+			stats_event->stats_buf_idxs[stream_info->stats_type] =
+				done_buf->buf_idx;
+			if (!stream_info->composite_flag) {
+				stats_event->stats_mask =
+					1 << stream_info->stats_type;
+				ISP_DBG("%s: stats event frame id: 0x%x\n",
+					__func__, buf_event.frame_id);
+				msm_isp_send_event(vfe_dev,
+					ISP_EVENT_STATS_NOTIFY +
+					stream_info->stats_type, &buf_event);
+			} else {
+				comp_stats_type_mask |=
+					1 << stream_info->stats_type;
+			}
+		}
+	}
+
+	if (comp_stats_type_mask) {
+		ISP_DBG("%s: composite stats event frame id: 0x%x mask: 0x%x\n",
+			__func__, buf_event.frame_id, comp_stats_type_mask);
+		stats_event->stats_mask = comp_stats_type_mask;
+		msm_isp_send_event(vfe_dev,
+			ISP_EVENT_COMP_STATS_NOTIFY, &buf_event);
+	}
+}
+
+int msm_isp_stats_create_stream(struct vfe_device *vfe_dev,
+	struct msm_vfe_stats_stream_request_cmd *stream_req_cmd)
+{
+	int rc = -1;
+	struct msm_vfe_stats_stream *stream_info = NULL;
+	struct msm_vfe_stats_shared_data *stats_data = &vfe_dev->stats_data;
+	uint32_t stats_idx;
+
+	if (!(vfe_dev->hw_info->stats_hw_info->stats_capability_mask &
+		(1 << stream_req_cmd->stats_type))) {
+		pr_err("%s: Stats type not supported\n", __func__);
+		return rc;
+	}
+
+	stats_idx = vfe_dev->hw_info->vfe_ops.stats_ops.
+		get_stats_idx(stream_req_cmd->stats_type);
+
+	stream_info = &stats_data->stream_info[stats_idx];
+	if (stream_info->state != STATS_AVALIABLE) {
+		pr_err("%s: Stats already requested\n", __func__);
+		return rc;
+	}
+
+	if (stream_req_cmd->framedrop_pattern >= MAX_SKIP) {
+		pr_err("%s: Invalid framedrop pattern\n", __func__);
+		return rc;
+	}
+
+	if (stream_req_cmd->irq_subsample_pattern >= MAX_SKIP) {
+		pr_err("%s: Invalid irq subsample pattern\n", __func__);
+		return rc;
+	}
+
+	stream_info->session_id = stream_req_cmd->session_id;
+	stream_info->stream_id = stream_req_cmd->stream_id;
+	stream_info->composite_flag = stream_req_cmd->composite_flag;
+	stream_info->stats_type = stream_req_cmd->stats_type;
+	stream_info->buffer_offset = stream_req_cmd->buffer_offset;
+	stream_info->framedrop_pattern = stream_req_cmd->framedrop_pattern;
+	stream_info->irq_subsample_pattern =
+		stream_req_cmd->irq_subsample_pattern;
+	stream_info->state = STATS_INACTIVE;
+
+	if ((vfe_dev->stats_data.stream_handle_cnt << 8) == 0)
+		vfe_dev->stats_data.stream_handle_cnt++;
+
+	stream_req_cmd->stream_handle =
+		(++vfe_dev->stats_data.stream_handle_cnt) << 8 | stats_idx;
+
+	stream_info->stream_handle = stream_req_cmd->stream_handle;
+	return 0;
+}
+
+int msm_isp_request_stats_stream(struct vfe_device *vfe_dev, void *arg)
+{
+	int rc = 0;
+	struct msm_vfe_stats_stream_request_cmd *stream_req_cmd = arg;
+	struct msm_vfe_stats_stream *stream_info = NULL;
+	struct msm_vfe_stats_shared_data *stats_data = &vfe_dev->stats_data;
+	uint32_t framedrop_period;
+	uint32_t stats_idx;
+
+	rc = msm_isp_stats_create_stream(vfe_dev, stream_req_cmd);
+	if (rc < 0) {
+		pr_err("%s: create stream failed\n", __func__);
+		return rc;
+	}
+
+	stats_idx = STATS_IDX(stream_req_cmd->stream_handle);
+	stream_info = &stats_data->stream_info[stats_idx];
+
+	framedrop_period = msm_isp_get_framedrop_period(
+	   stream_req_cmd->framedrop_pattern);
+
+	if (stream_req_cmd->framedrop_pattern == SKIP_ALL)
+		stream_info->framedrop_pattern = 0x0;
+	else
+		stream_info->framedrop_pattern = 0x1;
+	stream_info->framedrop_period = framedrop_period - 1;
+
+	if (!stream_info->composite_flag)
+		vfe_dev->hw_info->vfe_ops.stats_ops.
+			cfg_wm_irq_mask(vfe_dev, stream_info);
+
+	vfe_dev->hw_info->vfe_ops.stats_ops.cfg_wm_reg(vfe_dev, stream_info);
+	return rc;
+}
+
+int msm_isp_release_stats_stream(struct vfe_device *vfe_dev, void *arg)
+{
+	int rc = -1;
+	struct msm_vfe_stats_stream_cfg_cmd stream_cfg_cmd;
+	struct msm_vfe_stats_stream_release_cmd *stream_release_cmd = arg;
+	struct msm_vfe_stats_shared_data *stats_data = &vfe_dev->stats_data;
+	int stats_idx = STATS_IDX(stream_release_cmd->stream_handle);
+	struct msm_vfe_stats_stream *stream_info =
+		&stats_data->stream_info[stats_idx];
+
+	if (stream_info->state == STATS_AVALIABLE) {
+		pr_err("%s: stream already release\n", __func__);
+		return rc;
+	} else if (stream_info->state != STATS_INACTIVE) {
+		stream_cfg_cmd.enable = 0;
+		stream_cfg_cmd.num_streams = 1;
+		stream_cfg_cmd.stream_handle[0] =
+			stream_release_cmd->stream_handle;
+		rc = msm_isp_cfg_stats_stream(vfe_dev, &stream_cfg_cmd);
+	}
+
+	if (!stream_info->composite_flag)
+		vfe_dev->hw_info->vfe_ops.stats_ops.
+			clear_wm_irq_mask(vfe_dev, stream_info);
+
+	vfe_dev->hw_info->vfe_ops.stats_ops.clear_wm_reg(vfe_dev, stream_info);
+	memset(stream_info, 0, sizeof(struct msm_vfe_stats_stream));
+	return 0;
+}
+
+static int msm_isp_init_stats_ping_pong_reg(
+	struct vfe_device *vfe_dev,
+	struct msm_vfe_stats_stream *stream_info)
+{
+	int rc = 0;
+	stream_info->bufq_handle =
+		vfe_dev->buf_mgr->ops->get_bufq_handle(
+		vfe_dev->buf_mgr, stream_info->session_id,
+		stream_info->stream_id);
+	if (stream_info->bufq_handle == 0) {
+		pr_err("%s: no buf configured for stream: 0x%x\n",
+			__func__, stream_info->stream_handle);
+		return -EINVAL;
+	}
+
+	rc = msm_isp_stats_cfg_ping_pong_address(vfe_dev,
+		stream_info, VFE_PING_FLAG, NULL);
+	if (rc < 0) {
+		pr_err("%s: No free buffer for ping\n", __func__);
+		return rc;
+	}
+	rc = msm_isp_stats_cfg_ping_pong_address(vfe_dev,
+		stream_info, VFE_PONG_FLAG, NULL);
+	if (rc < 0) {
+		pr_err("%s: No free buffer for pong\n", __func__);
+		return rc;
+	}
+	return rc;
+}
+
+static void msm_isp_deinit_stats_ping_pong_reg(
+	struct vfe_device *vfe_dev,
+	struct msm_vfe_stats_stream *stream_info)
+{
+	int i;
+	struct msm_isp_buffer *buf;
+	for (i = 0; i < 2; i++) {
+		buf = stream_info->buf[i];
+		if (buf)
+			vfe_dev->buf_mgr->ops->put_buf(vfe_dev->buf_mgr,
+				buf->bufq_handle, buf->buf_idx);
+	}
+}
+
+void msm_isp_stats_stream_update(struct vfe_device *vfe_dev)
+{
+	int i;
+	uint32_t stats_mask = 0, comp_stats_mask = 0;
+	uint32_t enable = 0;
+	struct msm_vfe_stats_shared_data *stats_data = &vfe_dev->stats_data;
+	for (i = 0; i < vfe_dev->hw_info->stats_hw_info->num_stats_type; i++) {
+		if (stats_data->stream_info[i].state == STATS_START_PENDING ||
+				stats_data->stream_info[i].state ==
+					STATS_STOP_PENDING) {
+			stats_mask |= i;
+			enable = stats_data->stream_info[i].state ==
+				STATS_START_PENDING ? 1 : 0;
+			stats_data->stream_info[i].state =
+				stats_data->stream_info[i].state ==
+				STATS_START_PENDING ?
+				STATS_STARTING : STATS_STOPPING;
+			vfe_dev->hw_info->vfe_ops.stats_ops.enable_module(
+				vfe_dev, BIT(i), enable);
+			vfe_dev->hw_info->vfe_ops.stats_ops.cfg_comp_mask(
+			   vfe_dev, BIT(i), enable);
+		} else if (stats_data->stream_info[i].state == STATS_STARTING ||
+			stats_data->stream_info[i].state == STATS_STOPPING) {
+			if (stats_data->stream_info[i].composite_flag)
+				comp_stats_mask |= i;
+			if (stats_data->stream_info[i].state == STATS_STARTING)
+				atomic_add(BIT(i),
+					&stats_data->stats_comp_mask);
+			else
+				atomic_sub(BIT(i),
+					&stats_data->stats_comp_mask);
+			stats_data->stream_info[i].state =
+				stats_data->stream_info[i].state ==
+				STATS_STARTING ? STATS_ACTIVE : STATS_INACTIVE;
+		}
+	}
+	atomic_sub(1, &stats_data->stats_update);
+	if (!atomic_read(&stats_data->stats_update))
+		complete(&vfe_dev->stats_config_complete);
+}
+
+static int msm_isp_stats_wait_for_cfg_done(struct vfe_device *vfe_dev)
+{
+	int rc;
+	init_completion(&vfe_dev->stats_config_complete);
+	atomic_set(&vfe_dev->stats_data.stats_update, 2);
+	rc = wait_for_completion_interruptible_timeout(
+		&vfe_dev->stats_config_complete,
+		msecs_to_jiffies(VFE_MAX_CFG_TIMEOUT));
+	if (rc == 0) {
+		pr_err("%s: wait timeout\n", __func__);
+		rc = -1;
+	} else {
+		rc = 0;
+	}
+	return rc;
+}
+
+static int msm_isp_start_stats_stream(struct vfe_device *vfe_dev,
+	struct msm_vfe_stats_stream_cfg_cmd *stream_cfg_cmd)
+{
+	int i, rc = 0;
+	uint32_t stats_mask = 0, comp_stats_mask = 0, idx;
+	struct msm_vfe_stats_stream *stream_info;
+	struct msm_vfe_stats_shared_data *stats_data = &vfe_dev->stats_data;
+	for (i = 0; i < stream_cfg_cmd->num_streams; i++) {
+		idx = STATS_IDX(stream_cfg_cmd->stream_handle[i]);
+		stream_info = &stats_data->stream_info[idx];
+		if (stream_info->stream_handle !=
+				stream_cfg_cmd->stream_handle[i]) {
+			pr_err("%s: Invalid stream handle: 0x%x received\n",
+				__func__, stream_cfg_cmd->stream_handle[i]);
+			continue;
+		}
+		rc = msm_isp_init_stats_ping_pong_reg(vfe_dev, stream_info);
+		if (rc < 0) {
+			pr_err("%s: No buffer for stream%d\n", __func__, idx);
+			return rc;
+		}
+
+		if (vfe_dev->axi_data.src_info[VFE_PIX_0].active)
+			stream_info->state = STATS_START_PENDING;
+		else
+			stream_info->state = STATS_ACTIVE;
+
+		stats_data->num_active_stream++;
+		stats_mask |= 1 << idx;
+		if (stream_info->composite_flag)
+			comp_stats_mask |= 1 << idx;
+	}
+	if (vfe_dev->axi_data.src_info[VFE_PIX_0].active) {
+		rc = msm_isp_stats_wait_for_cfg_done(vfe_dev);
+	} else {
+		vfe_dev->hw_info->vfe_ops.stats_ops.enable_module(
+			vfe_dev, stats_mask, stream_cfg_cmd->enable);
+		atomic_add(comp_stats_mask, &stats_data->stats_comp_mask);
+		vfe_dev->hw_info->vfe_ops.stats_ops.cfg_comp_mask(
+		   vfe_dev, comp_stats_mask, 1);
+	}
+	return rc;
+}
+
+static int msm_isp_stop_stats_stream(struct vfe_device *vfe_dev,
+	struct msm_vfe_stats_stream_cfg_cmd *stream_cfg_cmd)
+{
+	int i, rc = 0;
+	uint32_t stats_mask = 0, comp_stats_mask = 0, idx;
+	struct msm_vfe_stats_stream *stream_info;
+	struct msm_vfe_stats_shared_data *stats_data = &vfe_dev->stats_data;
+	for (i = 0; i < stream_cfg_cmd->num_streams; i++) {
+		idx = STATS_IDX(stream_cfg_cmd->stream_handle[i]);
+		stream_info = &stats_data->stream_info[idx];
+		if (stream_info->stream_handle !=
+				stream_cfg_cmd->stream_handle[i]) {
+			pr_err("%s: Invalid stream handle: 0x%x received\n",
+				__func__, stream_cfg_cmd->stream_handle[i]);
+			continue;
+		}
+
+		if (vfe_dev->axi_data.src_info[VFE_PIX_0].active)
+			stream_info->state = STATS_STOP_PENDING;
+		else
+			stream_info->state = STATS_INACTIVE;
+
+		stats_data->num_active_stream--;
+		stats_mask |= 1 << idx;
+		if (stream_info->composite_flag)
+			comp_stats_mask |= 1 << idx;
+	}
+	if (vfe_dev->axi_data.src_info[VFE_PIX_0].active) {
+		rc = msm_isp_stats_wait_for_cfg_done(vfe_dev);
+	} else {
+		vfe_dev->hw_info->vfe_ops.stats_ops.enable_module(
+			vfe_dev, stats_mask, stream_cfg_cmd->enable);
+		atomic_sub(comp_stats_mask, &stats_data->stats_comp_mask);
+		vfe_dev->hw_info->vfe_ops.stats_ops.cfg_comp_mask(
+		   vfe_dev, comp_stats_mask, 0);
+	}
+
+	for (i = 0; i < stream_cfg_cmd->num_streams; i++) {
+		idx = STATS_IDX(stream_cfg_cmd->stream_handle[i]);
+		stream_info = &stats_data->stream_info[idx];
+		msm_isp_deinit_stats_ping_pong_reg(vfe_dev, stream_info);
+	}
+	return rc;
+}
+
+int msm_isp_cfg_stats_stream(struct vfe_device *vfe_dev, void *arg)
+{
+	int rc = 0;
+	struct msm_vfe_stats_stream_cfg_cmd *stream_cfg_cmd = arg;
+	if (vfe_dev->stats_data.num_active_stream == 0)
+		vfe_dev->hw_info->vfe_ops.stats_ops.cfg_ub(vfe_dev);
+
+	if (stream_cfg_cmd->enable)
+		rc = msm_isp_start_stats_stream(vfe_dev, stream_cfg_cmd);
+	else
+		rc = msm_isp_stop_stats_stream(vfe_dev, stream_cfg_cmd);
+
+	return rc;
+}

kernel/drivers/media/platform/msm/camera_v2/isp/msm_isp_stats_util.h

@@ -0,0 +1,25 @@
+/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#ifndef __MSM_ISP_STATS_UTIL_H__
+#define __MSM_ISP_STATS_UTIL_H__
+
+#include "msm_isp.h"
+#define STATS_IDX(idx) (idx & 0xFF)
+
+void msm_isp_process_stats_irq(struct vfe_device *vfe_dev,
+	uint32_t irq_status0, uint32_t irq_status1,
+	struct msm_isp_timestamp *ts);
+void msm_isp_stats_stream_update(struct vfe_device *vfe_dev);
+int msm_isp_cfg_stats_stream(struct vfe_device *vfe_dev, void *arg);
+int msm_isp_release_stats_stream(struct vfe_device *vfe_dev, void *arg);
+int msm_isp_request_stats_stream(struct vfe_device *vfe_dev, void *arg);
+#endif /* __MSM_ISP_STATS_UTIL_H__ */

kernel/drivers/media/platform/msm/camera_v2/isp/msm_isp_util.c

@@ -0,0 +1,936 @@
+/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/mutex.h>
+#include <linux/io.h>
+#include <media/v4l2-subdev.h>
+#include <linux/ratelimit.h>
+
+#include "msm.h"
+#include "msm_isp_util.h"
+#include "msm_isp_axi_util.h"
+#include "msm_isp_stats_util.h"
+#include "msm_camera_io_util.h"
+
+#define MAX_ISP_V4l2_EVENTS 100
+static DEFINE_MUTEX(bandwidth_mgr_mutex);
+static struct msm_isp_bandwidth_mgr isp_bandwidth_mgr;
+
+#define MSM_ISP_MIN_AB 300000000
+#define MSM_ISP_MIN_IB 450000000
+
+static struct msm_bus_vectors msm_isp_init_vectors[] = {
+	{
+		.src = MSM_BUS_MASTER_VFE,
+		.dst = MSM_BUS_SLAVE_EBI_CH0,
+		.ab  = 0,
+		.ib  = 0,
+	},
+};
+
+static struct msm_bus_vectors msm_isp_ping_vectors[] = {
+	{
+		.src = MSM_BUS_MASTER_VFE,
+		.dst = MSM_BUS_SLAVE_EBI_CH0,
+		.ab  = MSM_ISP_MIN_AB,
+		.ib  = MSM_ISP_MIN_IB,
+	},
+};
+
+static struct msm_bus_vectors msm_isp_pong_vectors[] = {
+	{
+		.src = MSM_BUS_MASTER_VFE,
+		.dst = MSM_BUS_SLAVE_EBI_CH0,
+		.ab  = MSM_ISP_MIN_AB,
+		.ib  = MSM_ISP_MIN_IB,
+	},
+};
+
+static struct msm_bus_paths msm_isp_bus_client_config[] = {
+	{
+		ARRAY_SIZE(msm_isp_init_vectors),
+		msm_isp_init_vectors,
+	},
+	{
+		ARRAY_SIZE(msm_isp_ping_vectors),
+		msm_isp_ping_vectors,
+	},
+	{
+		ARRAY_SIZE(msm_isp_pong_vectors),
+		msm_isp_pong_vectors,
+	},
+};
+
+static struct msm_bus_scale_pdata msm_isp_bus_client_pdata = {
+	msm_isp_bus_client_config,
+	ARRAY_SIZE(msm_isp_bus_client_config),
+	.name = "msm_camera_isp",
+};
+
+int msm_isp_init_bandwidth_mgr(enum msm_isp_hw_client client)
+{
+	int rc = 0;
+	mutex_lock(&bandwidth_mgr_mutex);
+	isp_bandwidth_mgr.client_info[client].active = 1;
+	if (isp_bandwidth_mgr.use_count++) {
+		mutex_unlock(&bandwidth_mgr_mutex);
+		return rc;
+	}
+	isp_bandwidth_mgr.bus_client =
+		msm_bus_scale_register_client(&msm_isp_bus_client_pdata);
+	if (!isp_bandwidth_mgr.bus_client) {
+		pr_err("%s: client register failed\n", __func__);
+		mutex_unlock(&bandwidth_mgr_mutex);
+		return -EINVAL;
+	}
+
+	isp_bandwidth_mgr.bus_vector_active_idx = 1;
+	msm_bus_scale_client_update_request(
+	   isp_bandwidth_mgr.bus_client,
+	   isp_bandwidth_mgr.bus_vector_active_idx);
+
+	mutex_unlock(&bandwidth_mgr_mutex);
+	return 0;
+}
+
+int msm_isp_update_bandwidth(enum msm_isp_hw_client client,
+	uint64_t ab, uint64_t ib)
+{
+	int i;
+	struct msm_bus_paths *path;
+	mutex_lock(&bandwidth_mgr_mutex);
+	if (!isp_bandwidth_mgr.use_count ||
+		!isp_bandwidth_mgr.bus_client) {
+		pr_err("%s: bandwidth manager inactive\n", __func__);
+		return -EINVAL;
+	}
+
+	isp_bandwidth_mgr.client_info[client].ab = ab;
+	isp_bandwidth_mgr.client_info[client].ib = ib;
+	ALT_VECTOR_IDX(isp_bandwidth_mgr.bus_vector_active_idx);
+	path =
+		&(msm_isp_bus_client_pdata.usecase[
+		  isp_bandwidth_mgr.bus_vector_active_idx]);
+	path->vectors[0].ab = MSM_ISP_MIN_AB;
+	path->vectors[0].ib = MSM_ISP_MIN_IB;
+	for (i = 0; i < MAX_ISP_CLIENT; i++) {
+		if (isp_bandwidth_mgr.client_info[i].active) {
+			path->vectors[0].ab +=
+				isp_bandwidth_mgr.client_info[i].ab;
+			path->vectors[0].ib +=
+				isp_bandwidth_mgr.client_info[i].ib;
+		}
+	}
+	msm_bus_scale_client_update_request(isp_bandwidth_mgr.bus_client,
+		isp_bandwidth_mgr.bus_vector_active_idx);
+	mutex_unlock(&bandwidth_mgr_mutex);
+	return 0;
+}
+
+void msm_isp_deinit_bandwidth_mgr(enum msm_isp_hw_client client)
+{
+	mutex_lock(&bandwidth_mgr_mutex);
+	memset(&isp_bandwidth_mgr.client_info[client], 0,
+		   sizeof(struct msm_isp_bandwidth_info));
+	if (--isp_bandwidth_mgr.use_count) {
+		mutex_unlock(&bandwidth_mgr_mutex);
+		return;
+	}
+
+	if (!isp_bandwidth_mgr.bus_client)
+		return;
+
+	msm_bus_scale_client_update_request(
+	   isp_bandwidth_mgr.bus_client, 0);
+	msm_bus_scale_unregister_client(isp_bandwidth_mgr.bus_client);
+	isp_bandwidth_mgr.bus_client = 0;
+	mutex_unlock(&bandwidth_mgr_mutex);
+}
+
+uint32_t msm_isp_get_framedrop_period(
+	enum msm_vfe_frame_skip_pattern frame_skip_pattern)
+{
+	switch (frame_skip_pattern) {
+	case NO_SKIP:
+	case EVERY_2FRAME:
+	case EVERY_3FRAME:
+	case EVERY_4FRAME:
+	case EVERY_5FRAME:
+	case EVERY_6FRAME:
+	case EVERY_7FRAME:
+	case EVERY_8FRAME:
+		return frame_skip_pattern + 1;
+	case EVERY_16FRAME:
+		return 16;
+		break;
+	case EVERY_32FRAME:
+		return 32;
+		break;
+	case SKIP_ALL:
+		return 1;
+	default:
+		return 1;
+	}
+	return 1;
+}
+
+static inline void msm_isp_get_timestamp(struct msm_isp_timestamp *time_stamp)
+{
+	struct timespec ts;
+	ktime_get_ts(&ts);
+	time_stamp->buf_time.tv_sec = ts.tv_sec;
+	time_stamp->buf_time.tv_usec = ts.tv_nsec/1000;
+	do_gettimeofday(&(time_stamp->event_time));
+}
+
+int msm_isp_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+	struct v4l2_event_subscription *sub)
+{
+	struct vfe_device *vfe_dev = v4l2_get_subdevdata(sd);
+	int rc = 0;
+	rc = v4l2_event_subscribe(fh, sub, MAX_ISP_V4l2_EVENTS);
+	if (rc == 0) {
+		if (sub->type == V4L2_EVENT_ALL) {
+			int i;
+
+			vfe_dev->axi_data.event_mask = 0;
+			for (i = 0; i < ISP_EVENT_MAX; i++)
+				vfe_dev->axi_data.event_mask |= (1 << i);
+		} else {
+			int event_idx = sub->type - ISP_EVENT_BASE;
+
+			vfe_dev->axi_data.event_mask |= (1 << event_idx);
+		}
+	}
+	return rc;
+}
+
+int msm_isp_unsubscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+	struct v4l2_event_subscription *sub)
+{
+	struct vfe_device *vfe_dev = v4l2_get_subdevdata(sd);
+	int rc = 0;
+
+	rc = v4l2_event_unsubscribe(fh, sub);
+	if (sub->type == V4L2_EVENT_ALL) {
+		vfe_dev->axi_data.event_mask = 0;
+	} else {
+		int event_idx = sub->type - ISP_EVENT_BASE;
+
+		vfe_dev->axi_data.event_mask &= ~(1 << event_idx);
+	}
+	return rc;
+}
+
+static int msm_isp_set_clk_rate(struct vfe_device *vfe_dev, long *rate)
+{
+	int rc = 0;
+	int clk_idx = vfe_dev->hw_info->vfe_clk_idx;
+	long round_rate =
+		clk_round_rate(vfe_dev->vfe_clk[clk_idx], *rate);
+	if (round_rate < 0) {
+		pr_err("%s: Invalid vfe clock rate\n", __func__);
+		return round_rate;
+	}
+
+	rc = clk_set_rate(vfe_dev->vfe_clk[clk_idx], round_rate);
+	if (rc < 0) {
+		pr_err("%s: Vfe set rate error\n", __func__);
+		return rc;
+	}
+	*rate = round_rate;
+	return 0;
+}
+
+int msm_isp_cfg_pix(struct vfe_device *vfe_dev,
+	struct msm_vfe_input_cfg *input_cfg)
+{
+	int rc = 0;
+	if (vfe_dev->axi_data.src_info[VFE_PIX_0].active) {
+		pr_err("%s: pixel path is active\n", __func__);
+		return -EINVAL;
+	}
+
+	vfe_dev->axi_data.src_info[VFE_PIX_0].pixel_clock =
+		input_cfg->input_pix_clk;
+	vfe_dev->axi_data.src_info[VFE_PIX_0].input_mux =
+		input_cfg->d.pix_cfg.input_mux;
+	vfe_dev->axi_data.src_info[VFE_PIX_0].width =
+		input_cfg->d.pix_cfg.camif_cfg.pixels_per_line;
+
+	rc = msm_isp_set_clk_rate(vfe_dev,
+		&vfe_dev->axi_data.src_info[VFE_PIX_0].pixel_clock);
+	if (rc < 0) {
+		pr_err("%s: clock set rate failed\n", __func__);
+		return rc;
+	}
+
+	vfe_dev->hw_info->vfe_ops.core_ops.cfg_camif(
+		vfe_dev, &input_cfg->d.pix_cfg);
+	return rc;
+}
+
+int msm_isp_cfg_rdi(struct vfe_device *vfe_dev,
+	struct msm_vfe_input_cfg *input_cfg)
+{
+	int rc = 0;
+	if (vfe_dev->axi_data.src_info[input_cfg->input_src].active) {
+		pr_err("%s: RAW%d path is active\n", __func__,
+			   input_cfg->input_src - VFE_RAW_0);
+		return -EINVAL;
+	}
+
+	vfe_dev->axi_data.src_info[input_cfg->input_src].pixel_clock =
+		input_cfg->input_pix_clk;
+	vfe_dev->hw_info->vfe_ops.core_ops.cfg_rdi_reg(
+		vfe_dev, &input_cfg->d.rdi_cfg, input_cfg->input_src);
+	return rc;
+}
+
+int msm_isp_cfg_input(struct vfe_device *vfe_dev, void *arg)
+{
+	int rc = 0;
+	struct msm_vfe_input_cfg *input_cfg = arg;
+
+	switch (input_cfg->input_src) {
+	case VFE_PIX_0:
+		rc = msm_isp_cfg_pix(vfe_dev, input_cfg);
+		break;
+	case VFE_RAW_0:
+	case VFE_RAW_1:
+	case VFE_RAW_2:
+		rc = msm_isp_cfg_rdi(vfe_dev, input_cfg);
+		break;
+	default:
+		pr_err("%s: Invalid input source\n", __func__);
+		rc = -EINVAL;
+	}
+	return rc;
+}
+
+long msm_isp_ioctl(struct v4l2_subdev *sd,
+	unsigned int cmd, void *arg)
+{
+	long rc = 0;
+	struct vfe_device *vfe_dev = v4l2_get_subdevdata(sd);
+
+	/* Use real time mutex for hard real-time ioctls such as
+	 * buffer operations and register updates.
+	 * Use core mutex for other ioctls that could take
+	 * longer time to complete such as start/stop ISP streams
+	 * which blocks until the hardware start/stop streaming
+	 */
+	ISP_DBG("%s cmd: %d\n", __func__, _IOC_TYPE(cmd));
+	switch (cmd) {
+	case VIDIOC_MSM_VFE_REG_CFG: {
+		mutex_lock(&vfe_dev->realtime_mutex);
+		rc = msm_isp_proc_cmd(vfe_dev, arg);
+		mutex_unlock(&vfe_dev->realtime_mutex);
+		break;
+	}
+	case VIDIOC_MSM_ISP_REQUEST_BUF:
+	case VIDIOC_MSM_ISP_ENQUEUE_BUF:
+	case VIDIOC_MSM_ISP_RELEASE_BUF: {
+		mutex_lock(&vfe_dev->realtime_mutex);
+		rc = msm_isp_proc_buf_cmd(vfe_dev->buf_mgr, cmd, arg);
+		mutex_unlock(&vfe_dev->realtime_mutex);
+		break;
+	}
+	case VIDIOC_MSM_ISP_REQUEST_STREAM:
+		mutex_lock(&vfe_dev->core_mutex);
+		rc = msm_isp_request_axi_stream(vfe_dev, arg);
+		mutex_unlock(&vfe_dev->core_mutex);
+		break;
+	case VIDIOC_MSM_ISP_RELEASE_STREAM:
+		mutex_lock(&vfe_dev->core_mutex);
+		rc = msm_isp_release_axi_stream(vfe_dev, arg);
+		mutex_unlock(&vfe_dev->core_mutex);
+		break;
+	case VIDIOC_MSM_ISP_CFG_STREAM:
+		mutex_lock(&vfe_dev->core_mutex);
+		rc = msm_isp_cfg_axi_stream(vfe_dev, arg);
+		mutex_unlock(&vfe_dev->core_mutex);
+		break;
+	case VIDIOC_MSM_ISP_INPUT_CFG:
+		mutex_lock(&vfe_dev->core_mutex);
+		rc = msm_isp_cfg_input(vfe_dev, arg);
+		mutex_unlock(&vfe_dev->core_mutex);
+		break;
+	case VIDIOC_MSM_ISP_SET_SRC_STATE:
+		mutex_lock(&vfe_dev->core_mutex);
+		msm_isp_set_src_state(vfe_dev, arg);
+		mutex_unlock(&vfe_dev->core_mutex);
+		break;
+	case VIDIOC_MSM_ISP_REQUEST_STATS_STREAM:
+		mutex_lock(&vfe_dev->core_mutex);
+		rc = msm_isp_request_stats_stream(vfe_dev, arg);
+		mutex_unlock(&vfe_dev->core_mutex);
+		break;
+	case VIDIOC_MSM_ISP_RELEASE_STATS_STREAM:
+		mutex_lock(&vfe_dev->core_mutex);
+		rc = msm_isp_release_stats_stream(vfe_dev, arg);
+		mutex_unlock(&vfe_dev->core_mutex);
+		break;
+	case VIDIOC_MSM_ISP_CFG_STATS_STREAM:
+		mutex_lock(&vfe_dev->core_mutex);
+		rc = msm_isp_cfg_stats_stream(vfe_dev, arg);
+		mutex_unlock(&vfe_dev->core_mutex);
+		break;
+	case VIDIOC_MSM_ISP_UPDATE_STREAM:
+		mutex_lock(&vfe_dev->core_mutex);
+		rc = msm_isp_update_axi_stream(vfe_dev, arg);
+		mutex_unlock(&vfe_dev->core_mutex);
+		break;
+	case MSM_SD_SHUTDOWN:
+		while (vfe_dev->vfe_open_cnt != 0)
+			msm_isp_close_node(sd, NULL);
+		break;
+
+	default:
+		pr_err("%s: Invalid ISP command\n", __func__);
+		rc = -EINVAL;
+	}
+	return rc;
+}
+
+static int msm_isp_send_hw_cmd(struct vfe_device *vfe_dev,
+	struct msm_vfe_reg_cfg_cmd *reg_cfg_cmd,
+	uint32_t *cfg_data, uint32_t cmd_len)
+{
+	switch (reg_cfg_cmd->cmd_type) {
+	case VFE_WRITE: {
+		if (resource_size(vfe_dev->vfe_mem) <
+			(reg_cfg_cmd->u.rw_info.reg_offset +
+			reg_cfg_cmd->u.rw_info.len)) {
+			pr_err("%s: Invalid length\n", __func__);
+			return -EINVAL;
+		}
+		msm_camera_io_memcpy(vfe_dev->vfe_base +
+			reg_cfg_cmd->u.rw_info.reg_offset,
+			cfg_data + reg_cfg_cmd->u.rw_info.cmd_data_offset/4,
+			reg_cfg_cmd->u.rw_info.len);
+		break;
+	}
+	case VFE_WRITE_MB: {
+		uint32_t *data_ptr = cfg_data +
+			reg_cfg_cmd->u.rw_info.cmd_data_offset/4;
+		msm_camera_io_w_mb(*data_ptr, vfe_dev->vfe_base +
+			reg_cfg_cmd->u.rw_info.reg_offset);
+		break;
+	}
+	case VFE_CFG_MASK: {
+		uint32_t temp;
+		temp = msm_camera_io_r(vfe_dev->vfe_base +
+			reg_cfg_cmd->u.mask_info.reg_offset);
+		temp &= ~reg_cfg_cmd->u.mask_info.mask;
+		temp |= reg_cfg_cmd->u.mask_info.val;
+		msm_camera_io_w(temp, vfe_dev->vfe_base +
+			reg_cfg_cmd->u.mask_info.reg_offset);
+		break;
+	}
+	case VFE_WRITE_DMI_16BIT:
+	case VFE_WRITE_DMI_32BIT:
+	case VFE_WRITE_DMI_64BIT: {
+		int i;
+		uint32_t *hi_tbl_ptr = NULL, *lo_tbl_ptr = NULL;
+		uint32_t hi_val, lo_val, lo_val1;
+		if (reg_cfg_cmd->cmd_type == VFE_WRITE_DMI_64BIT) {
+			if (reg_cfg_cmd->u.dmi_info.hi_tbl_offset +
+				reg_cfg_cmd->u.dmi_info.len > cmd_len) {
+				pr_err("Invalid Hi Table out of bounds\n");
+				return -EINVAL;
+			}
+			hi_tbl_ptr = cfg_data +
+				reg_cfg_cmd->u.dmi_info.hi_tbl_offset/4;
+		}
+
+		if (reg_cfg_cmd->u.dmi_info.lo_tbl_offset +
+			reg_cfg_cmd->u.dmi_info.len > cmd_len) {
+			pr_err("Invalid Lo Table out of bounds\n");
+			return -EINVAL;
+		}
+		lo_tbl_ptr = cfg_data +
+			reg_cfg_cmd->u.dmi_info.lo_tbl_offset/4;
+
+		for (i = 0; i < reg_cfg_cmd->u.dmi_info.len/4; i++) {
+			lo_val = *lo_tbl_ptr++;
+			if (reg_cfg_cmd->cmd_type == VFE_WRITE_DMI_16BIT) {
+				lo_val1 = lo_val & 0x0000FFFF;
+				lo_val = (lo_val & 0xFFFF0000)>>16;
+				msm_camera_io_w(lo_val1, vfe_dev->vfe_base +
+					vfe_dev->hw_info->dmi_reg_offset + 0x4);
+			} else if (reg_cfg_cmd->cmd_type ==
+					   VFE_WRITE_DMI_64BIT) {
+				lo_tbl_ptr++;
+				hi_val = *hi_tbl_ptr;
+				hi_tbl_ptr = hi_tbl_ptr + 2;
+				msm_camera_io_w(hi_val, vfe_dev->vfe_base +
+					vfe_dev->hw_info->dmi_reg_offset);
+			}
+			msm_camera_io_w(lo_val, vfe_dev->vfe_base +
+				vfe_dev->hw_info->dmi_reg_offset + 0x4);
+		}
+		break;
+	}
+	case VFE_READ_DMI_16BIT:
+	case VFE_READ_DMI_32BIT:
+	case VFE_READ_DMI_64BIT: {
+		int i;
+		uint32_t *hi_tbl_ptr = NULL, *lo_tbl_ptr = NULL;
+		uint32_t hi_val, lo_val, lo_val1;
+		if (reg_cfg_cmd->cmd_type == VFE_READ_DMI_64BIT) {
+			if (reg_cfg_cmd->u.dmi_info.hi_tbl_offset +
+				reg_cfg_cmd->u.dmi_info.len > cmd_len) {
+				pr_err("Invalid Hi Table out of bounds\n");
+				return -EINVAL;
+			}
+			hi_tbl_ptr = cfg_data +
+				reg_cfg_cmd->u.dmi_info.hi_tbl_offset/4;
+		}
+
+		if (reg_cfg_cmd->u.dmi_info.lo_tbl_offset +
+			reg_cfg_cmd->u.dmi_info.len > cmd_len) {
+			pr_err("Invalid Lo Table out of bounds\n");
+			return -EINVAL;
+		}
+		lo_tbl_ptr = cfg_data +
+			reg_cfg_cmd->u.dmi_info.lo_tbl_offset/4;
+
+		for (i = 0; i < reg_cfg_cmd->u.dmi_info.len/4; i++) {
+			if (reg_cfg_cmd->cmd_type == VFE_READ_DMI_64BIT) {
+				hi_val = msm_camera_io_r(vfe_dev->vfe_base +
+					vfe_dev->hw_info->dmi_reg_offset);
+				*hi_tbl_ptr++ = hi_val;
+			}
+
+			lo_val = msm_camera_io_r(vfe_dev->vfe_base +
+					vfe_dev->hw_info->dmi_reg_offset + 0x4);
+
+			if (reg_cfg_cmd->cmd_type == VFE_READ_DMI_16BIT) {
+				lo_val1 = msm_camera_io_r(vfe_dev->vfe_base +
+					vfe_dev->hw_info->dmi_reg_offset + 0x4);
+				lo_val |= lo_val1 << 16;
+			}
+			*lo_tbl_ptr++ = lo_val;
+		}
+		break;
+	}
+	case VFE_READ: {
+		int i;
+		uint32_t *data_ptr = cfg_data +
+			reg_cfg_cmd->u.rw_info.cmd_data_offset/4;
+		for (i = 0; i < reg_cfg_cmd->u.rw_info.len/4; i++)
+			*data_ptr++ = msm_camera_io_r(vfe_dev->vfe_base +
+				reg_cfg_cmd->u.rw_info.reg_offset++);
+		break;
+	}
+	}
+	return 0;
+}
+
+int msm_isp_proc_cmd(struct vfe_device *vfe_dev, void *arg)
+{
+	int rc = 0, i;
+	struct msm_vfe_cfg_cmd2 *proc_cmd = arg;
+	struct msm_vfe_reg_cfg_cmd *reg_cfg_cmd;
+	uint32_t *cfg_data;
+
+	reg_cfg_cmd = kzalloc(sizeof(struct msm_vfe_reg_cfg_cmd)*
+		proc_cmd->num_cfg, GFP_KERNEL);
+	if (!reg_cfg_cmd) {
+		pr_err("%s: reg_cfg alloc failed\n", __func__);
+		rc = -ENOMEM;
+		goto reg_cfg_failed;
+	}
+
+	cfg_data = kzalloc(proc_cmd->cmd_len, GFP_KERNEL);
+	if (!cfg_data) {
+		pr_err("%s: cfg_data alloc failed\n", __func__);
+		rc = -ENOMEM;
+		goto cfg_data_failed;
+	}
+
+	if (copy_from_user(reg_cfg_cmd,
+		(void __user *)(proc_cmd->cfg_cmd),
+		sizeof(struct msm_vfe_reg_cfg_cmd) * proc_cmd->num_cfg)) {
+		rc = -EFAULT;
+		goto copy_cmd_failed;
+	}
+
+	if (copy_from_user(cfg_data,
+			(void __user *)(proc_cmd->cfg_data),
+			proc_cmd->cmd_len)) {
+		rc = -EFAULT;
+		goto copy_cmd_failed;
+	}
+
+	for (i = 0; i < proc_cmd->num_cfg; i++)
+		msm_isp_send_hw_cmd(vfe_dev, &reg_cfg_cmd[i],
+			cfg_data, proc_cmd->cmd_len);
+
+	if (copy_to_user(proc_cmd->cfg_data,
+			cfg_data, proc_cmd->cmd_len)) {
+		rc = -EFAULT;
+		goto copy_cmd_failed;
+	}
+
+copy_cmd_failed:
+	kfree(cfg_data);
+cfg_data_failed:
+	kfree(reg_cfg_cmd);
+reg_cfg_failed:
+	return rc;
+}
+
+int msm_isp_send_event(struct vfe_device *vfe_dev,
+	uint32_t event_type,
+	struct msm_isp_event_data *event_data)
+{
+	struct v4l2_event isp_event;
+	memset(&isp_event, 0, sizeof(struct v4l2_event));
+	isp_event.id = 0;
+	isp_event.type = event_type;
+	memcpy(&isp_event.u.data[0], event_data,
+		sizeof(struct msm_isp_event_data));
+	v4l2_event_queue(vfe_dev->subdev.sd.devnode, &isp_event);
+	return 0;
+}
+
+#define CAL_WORD(width, M, N) ((width * M + N - 1) / N)
+
+int msm_isp_cal_word_per_line(uint32_t output_format,
+	uint32_t pixel_per_line)
+{
+	int val = -1;
+	switch (output_format) {
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SGRBG8:
+	case V4L2_PIX_FMT_SRGGB8:
+	case V4L2_PIX_FMT_QBGGR8:
+	case V4L2_PIX_FMT_QGBRG8:
+	case V4L2_PIX_FMT_QGRBG8:
+	case V4L2_PIX_FMT_QRGGB8:
+	case V4L2_PIX_FMT_JPEG:
+	case V4L2_PIX_FMT_META:
+		val = CAL_WORD(pixel_per_line, 1, 8);
+		break;
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_SGBRG10:
+	case V4L2_PIX_FMT_SGRBG10:
+	case V4L2_PIX_FMT_SRGGB10:
+		val = CAL_WORD(pixel_per_line, 5, 32);
+		break;
+	case V4L2_PIX_FMT_SBGGR12:
+	case V4L2_PIX_FMT_SGBRG12:
+	case V4L2_PIX_FMT_SGRBG12:
+	case V4L2_PIX_FMT_SRGGB12:
+		val = CAL_WORD(pixel_per_line, 3, 16);
+		break;
+	case V4L2_PIX_FMT_QBGGR10:
+	case V4L2_PIX_FMT_QGBRG10:
+	case V4L2_PIX_FMT_QGRBG10:
+	case V4L2_PIX_FMT_QRGGB10:
+		val = CAL_WORD(pixel_per_line, 1, 6);
+		break;
+	case V4L2_PIX_FMT_QBGGR12:
+	case V4L2_PIX_FMT_QGBRG12:
+	case V4L2_PIX_FMT_QGRBG12:
+	case V4L2_PIX_FMT_QRGGB12:
+		val = CAL_WORD(pixel_per_line, 1, 5);
+		break;
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV14:
+	case V4L2_PIX_FMT_NV41:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		val = CAL_WORD(pixel_per_line, 1, 8);
+		break;
+		/*TD: Add more image format*/
+	default:
+		pr_err("%s: Invalid output format\n", __func__);
+		break;
+	}
+	return val;
+}
+
+int msm_isp_get_bit_per_pixel(uint32_t output_format)
+{
+	switch (output_format) {
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SGRBG8:
+	case V4L2_PIX_FMT_SRGGB8:
+	case V4L2_PIX_FMT_QBGGR8:
+	case V4L2_PIX_FMT_QGBRG8:
+	case V4L2_PIX_FMT_QGRBG8:
+	case V4L2_PIX_FMT_QRGGB8:
+	case V4L2_PIX_FMT_JPEG:
+	case V4L2_PIX_FMT_META:
+		return 8;
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_SGBRG10:
+	case V4L2_PIX_FMT_SGRBG10:
+	case V4L2_PIX_FMT_SRGGB10:
+	case V4L2_PIX_FMT_QBGGR10:
+	case V4L2_PIX_FMT_QGBRG10:
+	case V4L2_PIX_FMT_QGRBG10:
+	case V4L2_PIX_FMT_QRGGB10:
+		return 10;
+	case V4L2_PIX_FMT_SBGGR12:
+	case V4L2_PIX_FMT_SGBRG12:
+	case V4L2_PIX_FMT_SGRBG12:
+	case V4L2_PIX_FMT_SRGGB12:
+	case V4L2_PIX_FMT_QBGGR12:
+	case V4L2_PIX_FMT_QGBRG12:
+	case V4L2_PIX_FMT_QGRBG12:
+	case V4L2_PIX_FMT_QRGGB12:
+		return 12;
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV14:
+	case V4L2_PIX_FMT_NV41:
+		return 8;
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		return 16;
+		/*TD: Add more image format*/
+	default:
+		pr_err("%s: Invalid output format\n", __func__);
+		break;
+	}
+	return -EINVAL;
+}
+
+void msm_isp_update_error_frame_count(struct vfe_device *vfe_dev)
+{
+	struct msm_vfe_error_info *error_info = &vfe_dev->error_info;
+	error_info->info_dump_frame_count++;
+	if (error_info->info_dump_frame_count == 0)
+		error_info->info_dump_frame_count++;
+}
+
+void msm_isp_process_error_info(struct vfe_device *vfe_dev)
+{
+	int i;
+	struct msm_vfe_error_info *error_info = &vfe_dev->error_info;
+	static DEFINE_RATELIMIT_STATE(rs,
+		DEFAULT_RATELIMIT_INTERVAL, DEFAULT_RATELIMIT_BURST);
+	static DEFINE_RATELIMIT_STATE(rs_stats,
+		DEFAULT_RATELIMIT_INTERVAL, DEFAULT_RATELIMIT_BURST);
+
+	if (error_info->error_count == 1 ||
+		!(error_info->info_dump_frame_count % 100)) {
+		vfe_dev->hw_info->vfe_ops.core_ops.
+			process_error_status(vfe_dev);
+		error_info->error_mask0 = 0;
+		error_info->error_mask1 = 0;
+		error_info->camif_status = 0;
+		error_info->violation_status = 0;
+		for (i = 0; i < MAX_NUM_STREAM; i++) {
+			if (error_info->stream_framedrop_count[i] != 0 &&
+				__ratelimit(&rs)) {
+				pr_err("%s: Stream[%d]: dropped %d frames\n",
+					__func__, i,
+					error_info->stream_framedrop_count[i]);
+				error_info->stream_framedrop_count[i] = 0;
+			}
+		}
+		for (i = 0; i < MSM_ISP_STATS_MAX; i++) {
+			if (error_info->stats_framedrop_count[i] != 0 &&
+				__ratelimit(&rs_stats)) {
+				pr_err("%s: Stats stream[%d]: dropped %d frames\n",
+					__func__, i,
+					error_info->stats_framedrop_count[i]);
+				error_info->stats_framedrop_count[i] = 0;
+			}
+		}
+	}
+}
+
+static inline void msm_isp_update_error_info(struct vfe_device *vfe_dev,
+	uint32_t error_mask0, uint32_t error_mask1)
+{
+	vfe_dev->error_info.error_mask0 |= error_mask0;
+	vfe_dev->error_info.error_mask1 |= error_mask1;
+	vfe_dev->error_info.error_count++;
+}
+
+irqreturn_t msm_isp_process_irq(int irq_num, void *data)
+{
+	unsigned long flags;
+	struct msm_vfe_tasklet_queue_cmd *queue_cmd;
+	struct vfe_device *vfe_dev = (struct vfe_device *) data;
+	uint32_t irq_status0, irq_status1;
+	uint32_t error_mask0, error_mask1;
+
+	vfe_dev->hw_info->vfe_ops.irq_ops.
+		read_irq_status(vfe_dev, &irq_status0, &irq_status1);
+	vfe_dev->hw_info->vfe_ops.core_ops.
+		get_error_mask(&error_mask0, &error_mask1);
+	error_mask0 &= irq_status0;
+	error_mask1 &= irq_status1;
+	irq_status0 &= ~error_mask0;
+	irq_status1 &= ~error_mask1;
+	if ((error_mask0 != 0) || (error_mask1 != 0))
+		msm_isp_update_error_info(vfe_dev, error_mask0, error_mask1);
+
+	if ((irq_status0 == 0) && (irq_status1 == 0) &&
+		(!((error_mask0 != 0) || (error_mask1 != 0)) &&
+		 vfe_dev->error_info.error_count == 1)) {
+		ISP_DBG("%s: irq_status0 & 1 are both 0!\n", __func__);
+		return IRQ_HANDLED;
+	}
+
+	spin_lock_irqsave(&vfe_dev->tasklet_lock, flags);
+	queue_cmd = &vfe_dev->tasklet_queue_cmd[vfe_dev->taskletq_idx];
+	if (queue_cmd->cmd_used) {
+		pr_err_ratelimited("%s: Tasklet queue overflow: %d\n",
+			__func__, vfe_dev->pdev->id);
+		list_del(&queue_cmd->list);
+	} else {
+		atomic_add(1, &vfe_dev->irq_cnt);
+	}
+	queue_cmd->vfeInterruptStatus0 = irq_status0;
+	queue_cmd->vfeInterruptStatus1 = irq_status1;
+	msm_isp_get_timestamp(&queue_cmd->ts);
+	queue_cmd->cmd_used = 1;
+	vfe_dev->taskletq_idx =
+		(vfe_dev->taskletq_idx + 1) % MSM_VFE_TASKLETQ_SIZE;
+	list_add_tail(&queue_cmd->list, &vfe_dev->tasklet_q);
+	spin_unlock_irqrestore(&vfe_dev->tasklet_lock, flags);
+	tasklet_schedule(&vfe_dev->vfe_tasklet);
+	return IRQ_HANDLED;
+}
+
+void msm_isp_do_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	struct vfe_device *vfe_dev = (struct vfe_device *) data;
+	struct msm_vfe_irq_ops *irq_ops = &vfe_dev->hw_info->vfe_ops.irq_ops;
+	struct msm_vfe_tasklet_queue_cmd *queue_cmd;
+	struct msm_isp_timestamp ts;
+	uint32_t irq_status0, irq_status1;
+	while (atomic_read(&vfe_dev->irq_cnt)) {
+		spin_lock_irqsave(&vfe_dev->tasklet_lock, flags);
+		queue_cmd = list_first_entry(&vfe_dev->tasklet_q,
+		struct msm_vfe_tasklet_queue_cmd, list);
+		if (!queue_cmd) {
+			atomic_set(&vfe_dev->irq_cnt, 0);
+			spin_unlock_irqrestore(&vfe_dev->tasklet_lock, flags);
+			return;
+		}
+		atomic_sub(1, &vfe_dev->irq_cnt);
+		list_del(&queue_cmd->list);
+		queue_cmd->cmd_used = 0;
+		irq_status0 = queue_cmd->vfeInterruptStatus0;
+		irq_status1 = queue_cmd->vfeInterruptStatus1;
+		ts = queue_cmd->ts;
+		spin_unlock_irqrestore(&vfe_dev->tasklet_lock, flags);
+		ISP_DBG("%s: status0: 0x%x status1: 0x%x\n",
+			__func__, irq_status0, irq_status1);
+		irq_ops->process_reset_irq(vfe_dev,
+			irq_status0, irq_status1);
+		irq_ops->process_halt_irq(vfe_dev,
+			irq_status0, irq_status1);
+		irq_ops->process_camif_irq(vfe_dev,
+			irq_status0, irq_status1, &ts);
+		irq_ops->process_axi_irq(vfe_dev,
+			irq_status0, irq_status1, &ts);
+		irq_ops->process_stats_irq(vfe_dev,
+			irq_status0, irq_status1, &ts);
+		irq_ops->process_reg_update(vfe_dev,
+			irq_status0, irq_status1, &ts);
+		msm_isp_process_error_info(vfe_dev);
+	}
+}
+
+void msm_isp_set_src_state(struct vfe_device *vfe_dev, void *arg)
+{
+	struct msm_vfe_axi_src_state *src_state = arg;
+	vfe_dev->axi_data.src_info[src_state->input_src].active =
+	src_state->src_active;
+}
+
+int msm_isp_open_node(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct vfe_device *vfe_dev = v4l2_get_subdevdata(sd);
+	long rc;
+	ISP_DBG("%s\n", __func__);
+
+	mutex_lock(&vfe_dev->realtime_mutex);
+	mutex_lock(&vfe_dev->core_mutex);
+	if (vfe_dev->vfe_open_cnt == 1) {
+		pr_err("VFE already open\n");
+		mutex_unlock(&vfe_dev->core_mutex);
+		mutex_unlock(&vfe_dev->realtime_mutex);
+		return -ENODEV;
+	}
+
+	if (vfe_dev->hw_info->vfe_ops.core_ops.init_hw(vfe_dev) < 0) {
+		pr_err("%s: init hardware failed\n", __func__);
+		mutex_unlock(&vfe_dev->core_mutex);
+		mutex_unlock(&vfe_dev->realtime_mutex);
+		return -EBUSY;
+	}
+
+	rc = vfe_dev->hw_info->vfe_ops.core_ops.reset_hw(vfe_dev);
+	if (rc <= 0) {
+		pr_err("%s: reset timeout\n", __func__);
+		mutex_unlock(&vfe_dev->core_mutex);
+		mutex_unlock(&vfe_dev->realtime_mutex);
+		return -EINVAL;
+	}
+	vfe_dev->vfe_hw_version = msm_camera_io_r(vfe_dev->vfe_base);
+	ISP_DBG("%s: HW Version: 0x%x\n", __func__, vfe_dev->vfe_hw_version);
+
+	vfe_dev->hw_info->vfe_ops.core_ops.init_hw_reg(vfe_dev);
+
+	vfe_dev->buf_mgr->ops->buf_mgr_init(vfe_dev->buf_mgr, "msm_isp", 28);
+
+	memset(&vfe_dev->axi_data, 0, sizeof(struct msm_vfe_axi_shared_data));
+	memset(&vfe_dev->stats_data, 0,
+		sizeof(struct msm_vfe_stats_shared_data));
+	memset(&vfe_dev->error_info, 0, sizeof(vfe_dev->error_info));
+	vfe_dev->axi_data.hw_info = vfe_dev->hw_info->axi_hw_info;
+	vfe_dev->vfe_open_cnt++;
+	vfe_dev->taskletq_idx = 0;
+	mutex_unlock(&vfe_dev->core_mutex);
+	mutex_unlock(&vfe_dev->realtime_mutex);
+	return 0;
+}
+
+int msm_isp_close_node(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	long rc;
+	struct vfe_device *vfe_dev = v4l2_get_subdevdata(sd);
+	ISP_DBG("%s\n", __func__);
+	mutex_lock(&vfe_dev->realtime_mutex);
+	mutex_lock(&vfe_dev->core_mutex);
+	if (vfe_dev->vfe_open_cnt == 0) {
+		pr_err("%s: Invalid close\n", __func__);
+		mutex_unlock(&vfe_dev->core_mutex);
+		mutex_unlock(&vfe_dev->realtime_mutex);
+		return -ENODEV;
+	}
+
+	rc = vfe_dev->hw_info->vfe_ops.axi_ops.halt(vfe_dev);
+	if (rc <= 0)
+		pr_err("%s: halt timeout rc=%ld\n", __func__, rc);
+
+	vfe_dev->buf_mgr->ops->buf_mgr_deinit(vfe_dev->buf_mgr);
+	vfe_dev->hw_info->vfe_ops.core_ops.release_hw(vfe_dev);
+	vfe_dev->vfe_open_cnt--;
+	mutex_unlock(&vfe_dev->core_mutex);
+	mutex_unlock(&vfe_dev->realtime_mutex);
+	return 0;
+}

kernel/drivers/media/platform/msm/camera_v2/isp/msm_isp_util.h

@@ -0,0 +1,74 @@
+/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#ifndef __MSM_ISP_UTIL_H__
+#define __MSM_ISP_UTIL_H__
+
+#include "msm_isp.h"
+
+/* #define CONFIG_MSM_ISP_DBG 1 */
+
+#ifdef CONFIG_MSM_ISP_DBG
+#define ISP_DBG(fmt, args...) printk(fmt, ##args)
+#else
+#define ISP_DBG(fmt, args...) pr_debug(fmt, ##args)
+#endif
+
+#define ALT_VECTOR_IDX(x) {x = 3 - x; }
+struct msm_isp_bandwidth_info {
+	uint32_t active;
+	uint64_t ab;
+	uint64_t ib;
+};
+
+enum msm_isp_hw_client {
+	ISP_VFE0,
+	ISP_VFE1,
+	ISP_CPP,
+	MAX_ISP_CLIENT,
+};
+
+struct msm_isp_bandwidth_mgr {
+	uint32_t bus_client;
+	uint32_t bus_vector_active_idx;
+	uint32_t use_count;
+	struct msm_isp_bandwidth_info client_info[MAX_ISP_CLIENT];
+};
+
+uint32_t msm_isp_get_framedrop_period(
+	enum msm_vfe_frame_skip_pattern frame_skip_pattern);
+
+int msm_isp_init_bandwidth_mgr(enum msm_isp_hw_client client);
+int msm_isp_update_bandwidth(enum msm_isp_hw_client client,
+	uint64_t ab, uint64_t ib);
+void msm_isp_deinit_bandwidth_mgr(enum msm_isp_hw_client client);
+
+int msm_isp_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+	struct v4l2_event_subscription *sub);
+
+int msm_isp_unsubscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+	struct v4l2_event_subscription *sub);
+
+int msm_isp_proc_cmd(struct vfe_device *vfe_dev, void *arg);
+int msm_isp_send_event(struct vfe_device *vfe_dev,
+	uint32_t type, struct msm_isp_event_data *event_data);
+int msm_isp_cal_word_per_line(uint32_t output_format,
+	uint32_t pixel_per_line);
+int msm_isp_get_bit_per_pixel(uint32_t output_format);
+irqreturn_t msm_isp_process_irq(int irq_num, void *data);
+void msm_isp_set_src_state(struct vfe_device *vfe_dev, void *arg);
+void msm_isp_do_tasklet(unsigned long data);
+void msm_isp_update_error_frame_count(struct vfe_device *vfe_dev);
+void msm_isp_process_error_info(struct vfe_device *vfe_dev);
+int msm_isp_open_node(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh);
+int msm_isp_close_node(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh);
+long msm_isp_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg);
+#endif /* __MSM_ISP_UTIL_H__ */