Moved, renamed, and deleted files

The original directory structure was scattered and unorganized.
Changes are basically to make it look like kernel structure.

1 files changed, 1264 insertions, 0 deletions

diff --git a/drivers/media/video/wmt_v4l2/sensors/wmt-cmos.c b/drivers/media/video/wmt_v4l2/sensors/wmt-cmos.c
new file mode 100755
index 00000000..a6ed5e50
--- /dev/null
+++ b/drivers/media/video/wmt_v4l2/sensors/wmt-cmos.c
@@ -0,0 +1,1264 @@
+/*++
+ * linux/drivers/media/video/wmt_v4l2/cmos/wmt-cmos.c
+ * WonderMedia v4l cmos device driver
+ *
+ * Copyright c 2010  WonderMedia  Technologies, Inc.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * WonderMedia Technologies, Inc.
+ * 4F, 533, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C
+ --*/
+
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/major.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/i2c.h>
+
+#include <linux/wmt-mb.h>
+#include <mach/wmt_env.h>
+
+
+#include "wmt-cmos.h"
+#include "cmos-subdev.h"
+#include "../wmt-vid.h"
+
+#include "flash/flash.h"
+
+#define THE_MB_USER "CMOS-MB"
+
+#undef pr_err
+#define pr_err(fmt, args...)	printk("[" THE_MB_USER "] " "%s, %d:" fmt, __func__, __LINE__, ##args)
+
+//#define CMOS_REG_TRACE
+#ifdef CMOS_REG_TRACE
+#define CMOS_REG_SET32(addr, val)  \
+	PRINT("REG_SET:0x%x -> 0x%0x\n", addr, val);\
+	REG32_VAL(addr) = (val)
+#else
+#define CMOS_REG_SET32(addr, val)      REG32_VAL(addr) = (val)
+#endif
+
+//#define CMOS_DEBUG    /* Flag to enable debug message */
+//#define CMOS_TRACE
+
+#ifdef CMOS_DEBUG
+#define DBG_MSG(fmt, args...)    printk("{%s} " fmt, __FUNCTION__ , ## args)
+#else
+#define DBG_MSG(fmt, args...)
+#endif
+
+#ifdef CMOS_TRACE
+#define TRACE(fmt, args...)      printk("{%s}:  " fmt, __FUNCTION__ , ## args)
+#else
+#define TRACE(fmt, args...)
+#endif
+
+#define MAX_FB_IN_QUEUE		10
+#define NUM_INPUTS		3
+
+#define CAMERA_TYPE_NA          0   /* not avaliable */
+#define CAMERA_TYPE_CMOS        1   /* CMOS */
+#define CAMERA_TYPE_USB         2   /* USB  */
+
+struct regulator *cam_vccvid = NULL;
+struct regulator *cam_dvdd = NULL;
+
+typedef enum {
+	STS_CMOS_READY    = 0,
+	STS_CMOS_WAIT     = 0x0001,
+	STS_CMOS_RUNNING  = 0x0002,
+	STS_CMOS_FB_DONE  = 0x0004
+} cmos_status;
+
+typedef struct {
+	VID_FB_M;
+	struct list_head list;
+} cmos_fb_t;
+
+struct cmos_drvinfo {
+	struct list_head  head;
+
+	unsigned int  frame_size;
+	unsigned int  width;
+	unsigned int  height;
+
+	unsigned int  dft_y_addr;
+	unsigned int  dft_c_addr;
+
+	cmos_fb_t     fb_pool[MAX_FB_IN_QUEUE];
+	unsigned int  fb_cnt;
+
+	unsigned int  streamoff;
+	unsigned int  dqbuf;
+
+	cmos_status   status;
+};
+
+// sync with UbootParam.h in Hal.
+enum {
+    CAMERA_CAP_FLASH        = 1 << 0,
+    CAMERA_CAP_FLASH_AUTO   = 1 << 1,
+    CAMERA_CAP_HDR          = 1 << 2,
+};
+
+struct cmos_dev {
+	int	type;
+	int	pwdn_gpio;
+	int	pwdn_pol;
+	int	mirror;
+	u32	cap;
+
+	int	id;
+	struct cmos_subdev *sd;
+};
+
+struct cmos_info {
+	int	input;
+	int	input_count;
+
+	struct cmos_drvinfo	drvinfo;
+	struct cmos_dev		cd[NUM_INPUTS];
+	struct flash_dev	*fl;
+
+	struct mutex	lock;
+};
+
+static inline struct cmos_subdev *current_subdev(struct cmos_info *info)
+{
+	if (!info || info->input >= NUM_INPUTS)
+		return NULL;
+
+	return info->cd[info->input].sd;
+}
+
+static DECLARE_WAIT_QUEUE_HEAD(cmos_wait);
+static spinlock_t cmos_lock;
+static int cmos_dev_ref;
+
+static int cmos_dev_setup(struct cmos_info *info)
+{
+	struct cmos_dev *cd;
+	int rc = 0;
+	int i;
+
+	for (i = 0; i < info->input_count; i++) {
+		cd = &info->cd[i];
+		if (cd->type != CAMERA_TYPE_CMOS)
+			continue;
+		rc = gpio_request(cd->pwdn_gpio, "cmos pwdn");
+		if (rc) {
+			pr_err("cmos gpio%d request failed\n", cd->pwdn_gpio);
+			goto err_gpio_req;
+		}
+		gpio_direction_output(cd->pwdn_gpio, !cd->pwdn_pol);
+	}
+	return 0;
+
+err_gpio_req:
+	while (i--)
+		gpio_free(info->cd[i].pwdn_gpio);
+	return rc;
+}
+
+static void cmos_dev_power_up(struct cmos_dev *cd)
+{
+	if (cd->pwdn_gpio > 0) {
+		printk(KERN_DEBUG "%s, gpio-%d\n", __func__, cd->pwdn_gpio);
+		gpio_direction_output(cd->pwdn_gpio, cd->pwdn_pol);
+		msleep(20);
+	}
+}
+
+static void cmos_dev_power_down(struct cmos_dev *cd)
+{
+	if (cd->pwdn_gpio > 0) {
+		printk(KERN_DEBUG "%s, gpio-%d\n", __func__, cd->pwdn_gpio);
+		gpio_direction_output(cd->pwdn_gpio, !cd->pwdn_pol);
+	}
+}
+
+static void cmos_dev_release(struct cmos_info *info)
+{
+	struct cmos_dev *cd;
+	int i;
+
+	for (i = 0; i < info->input_count; i++) {
+		cd = &info->cd[i];
+		if (cd->type != CAMERA_TYPE_CMOS)
+			continue;
+		cmos_dev_power_down(cd);
+		gpio_free(cd->pwdn_gpio);
+	}
+}
+
+static int parse_camera_param(struct cmos_info *info)
+{
+	static char env[] = "wmt.camera.param";
+	struct cmos_dev cd[2];
+	char buf[64];
+	size_t l = sizeof(buf);
+	int i, n;
+	int rc;
+
+	rc = wmt_getsyspara(env, buf, &l);
+	if (rc) {
+		pr_err("Invalid param, please set %s\n", env);
+		return -EINVAL;
+	}
+
+	memset(cd, 0, sizeof(cd));
+	sscanf(buf, "%d:%d:%d:%d:%d:%d:%d:%d:%d:%x",
+	       &cd[1].type, &cd[1].pwdn_gpio, &cd[1].pwdn_pol, &cd[1].mirror, &cd[1].cap,
+	       &cd[0].type, &cd[0].pwdn_gpio, &cd[0].pwdn_pol, &cd[0].mirror, &cd[0].cap);
+
+	for (n = 0, i = 0; i < ARRAY_SIZE(cd); i++) {
+		struct cmos_dev *c = &cd[i];
+
+		if (c->type != CAMERA_TYPE_CMOS && c->type != CAMERA_TYPE_USB)
+			continue;
+
+		info->cd[n] = cd[i];
+		info->cd[n].id = n;
+
+		pr_info("#%d: type %d, pwdn_gpio %d, pwdn_pol %d, mirror %d, cap 0x%x\n",
+			n, c->type, c->pwdn_gpio, c->pwdn_pol, c->mirror, c->cap);
+		n++;
+	}
+	info->input_count = n;
+
+	return (n == 0) ? -EINVAL : 0;
+}
+
+static int cam_enable(struct cmos_drvinfo *drv, bool en)
+{
+	drv->streamoff = en ? 0 : 1;
+
+	CMOS_REG_SET32(REG_VID_CMOS_EN, en);	/* enable CMOS */
+
+	return 0;
+}
+
+static int print_queue(struct cmos_drvinfo *drv)
+{
+#ifdef CMOS_DEBUG
+	struct list_head *next;
+	cmos_fb_t  *fb;
+
+	TRACE("Enter\n");
+
+	list_for_each(next, &drv->head) {
+		fb = (cmos_fb_t *) list_entry(next, cmos_fb_t, list);
+		DBG_MSG("[%d] y_addr: 0x%x, c_addr: 0x%x\n",
+			fb->id, fb->y_addr, fb->c_addr );
+	}
+	TRACE("Leave\n");
+#endif
+	return 0;
+}
+
+static int put_queue(struct cmos_drvinfo *drv, cmos_fb_t *fb_in)
+{
+	TRACE("Enter\n");
+
+	list_add_tail(&fb_in->list, &drv->head);
+	wmb();
+	print_queue(drv);
+
+	TRACE("Leave\n");
+
+	return 0;
+}
+
+static int pop_queue(struct cmos_drvinfo *drv, cmos_fb_t *fb_in)
+{
+	TRACE("Enter\n");
+
+	list_del(&fb_in->list);
+	wmb();
+	print_queue(drv);
+
+	TRACE("Leave\n");
+
+	return 0;
+}
+
+static int cmos_cam_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
+{
+	TRACE("Enter\n");
+
+	strlcpy(cap->driver, "wm88xx", sizeof(cap->driver));
+
+	cap->version = KERNEL_VERSION(0, 0, 1);
+
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
+		V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+
+	TRACE("Leave\n");
+	return 0;
+}
+
+static int cmos_cam_cropcap(struct file *file, void *fh, struct v4l2_cropcap *cropcap)
+{
+	pr_err("Not support now!\n");
+	return -EINVAL;
+}
+
+struct wmt_fmt {
+	char  *name;
+	u32   fourcc;          /* v4l2 format id */
+	int   depth;
+};
+
+static struct wmt_fmt formats[] = {
+	{
+		.name     = "4:2:2, packed, YUYV",
+		.fourcc   = V4L2_PIX_FMT_YUYV,
+		.depth    = 16,
+	},
+};
+
+static int cmos_cam_enum_fmt_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct wmt_fmt *fmt;
+
+	if (f->index >= ARRAY_SIZE(formats))
+		return -EINVAL;
+
+	fmt = &formats[f->index];
+
+	strlcpy(f->description, fmt->name, sizeof(f->description));
+	f->pixelformat = fmt->fourcc;
+	return 0;
+}
+
+static int cmos_cam_enum_framesizes(struct file *file, void *fh,
+				    struct v4l2_frmsizeenum *fsize)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_subdev *sd = current_subdev(info);
+	int ret;
+
+	mutex_lock(&info->lock);
+	ret = cmos_subdev_call(sd, enum_framesizes, fsize);
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int cmos_cam_g_fmt_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_drvinfo *drv = &info->drvinfo;
+
+	f->fmt.pix.width = drv->width;
+	f->fmt.pix.height = drv->height;
+	return 0;
+}
+
+#define pixfmtstr(x) (x) & 0xff, ((x) >> 8) & 0xff, ((x) >> 16) & 0xff, \
+	((x) >> 24) & 0xff
+
+static int cmos_cam_s_fmt_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_drvinfo *drv = &info->drvinfo;
+	struct cmos_subdev *sd = current_subdev(info);
+	struct v4l2_mbus_framefmt mf;
+	int rc;
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+
+	printk(KERN_DEBUG "%s: S_FMT %dx%d, fmt %c%c%c%c\n",
+	       sd->name, f->fmt.pix.width, f->fmt.pix.height,
+	       pixfmtstr(f->fmt.pix.pixelformat));
+
+	mf.width	= f->fmt.pix.width;
+	mf.height	= f->fmt.pix.height;
+	mf.field	= f->fmt.pix.field;
+	mf.colorspace	= f->fmt.pix.colorspace;
+
+	rc = cmos_subdev_call(sd, s_mbus_fmt, &mf);
+	if (rc) {
+		pr_err("%s, %s: s_mbus_fmt fail\n", __func__, sd->name);
+		goto out;
+	}
+
+	/* host set fmt */
+	rc = wmt_vid_set_mode(mf.width, mf.height, f->fmt.pix.pixelformat);
+	if (rc) {
+		goto out;
+	}
+
+	drv->width	= mf.width;
+	drv->height	= mf.height;
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_RGB32:	// *4 for  ARGB display frambuffer
+		drv->frame_size = drv->width * drv->height * 4;
+		break;
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV12:
+		drv->frame_size = (drv->width * drv->height * 3) / 2;
+		break;
+	case V4L2_PIX_FMT_YUYV:		// YC422
+		drv->frame_size = drv->width * drv->height * 2;
+		break;
+	default:
+		drv->frame_size = 0;
+		rc = -EINVAL;
+		break;
+	}
+
+out:
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	return rc;
+}
+
+static int cmos_cam_try_fmt_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_subdev *sd = current_subdev(info);
+	struct v4l2_mbus_framefmt mf;
+	int ret;
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+
+	mf.width	= f->fmt.pix.width;
+	mf.height	= f->fmt.pix.height;
+	mf.field	= f->fmt.pix.field;
+	mf.colorspace	= f->fmt.pix.colorspace;
+
+	ret = cmos_subdev_call(sd, try_mbus_fmt, &mf);
+	if (!ret) {
+		f->fmt.pix.width = mf.width;
+		f->fmt.pix.height = mf.height;
+	}
+
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	return ret;
+}
+
+static int cmos_cam_reqbufs(struct file *file, void *fh, struct v4l2_requestbuffers *rb)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_drvinfo *drv = &info->drvinfo;
+	cmos_fb_t *fb;
+	int  i, j;
+	int ret = 0;
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+
+	if ((rb->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
+	    (rb->memory != V4L2_MEMORY_MMAP) ||
+	    (rb->count > MAX_FB_IN_QUEUE) ) {
+		pr_err("rb->type: 0x%x, rb->memory: 0x%x, rb->count: %d\n",
+		       rb->type, rb->memory, rb->count);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (!drv->streamoff) {
+		pr_err("reqbufs: streaming active\n");
+		ret = -EBUSY;
+		goto out;
+	}
+
+	if (rb->count == 0) {
+		/* Free memory */
+		for (i = 0; i < drv->fb_cnt; i++) {
+			fb = &drv->fb_pool[i];
+			if (fb->y_addr) {
+				mb_free(fb->y_addr);
+				memset(fb, 0, sizeof(cmos_fb_t));
+			}
+		}
+		drv->fb_cnt = 0;
+		INIT_LIST_HEAD(&drv->head);
+		goto out;
+	}
+
+	drv->fb_cnt = rb->count;
+
+	DBG_MSG(" Frame Size: %d Bytes\n", drv->frame_size);
+	DBG_MSG(" fb_cnt:     %d\n", drv->fb_cnt);
+
+	/* Allocate memory */
+	for (i = 0; i < drv->fb_cnt; i++) {
+		fb = &drv->fb_pool[i];
+		fb->y_addr = mb_alloc(drv->frame_size);
+		DBG_MSG("%d CMOS MB_allocate size %d\n", i,drv->frame_size);
+		if (fb->y_addr == 0) {
+			/* Allocate MB memory size fail */
+			pr_err("[%d] >> Allocate MB memory (%d) fail!\n", i, drv->frame_size);
+			for (j=0; j<i; j++) {
+				fb = &drv->fb_pool[j];
+				mb_free(fb->y_addr);
+			}
+			ret = -EINVAL;
+			goto out;
+		}
+		fb->c_addr = fb->y_addr + drv->width * drv->height;
+		fb->id = i;
+		fb->is_busy = 0;
+		fb->done    = 0;
+		DBG_MSG("[%d] fb: 0x%x, y_addr: 0x%x, c_addr: 0x%x\n",
+			i, (unsigned int)fb, fb->y_addr, fb->c_addr);
+	}
+
+out:
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	return ret;
+}
+
+static int cmos_cam_querybuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_drvinfo *drv = &info->drvinfo;
+
+	TRACE("Enter (index: %d)\n", b->index);
+	mutex_lock(&info->lock);
+
+	if (b->index < MAX_FB_IN_QUEUE) {
+		cmos_fb_t *fb = &drv->fb_pool[b->index];
+		b->length   = drv->frame_size;
+		b->m.offset = fb->y_addr;
+	} else {
+		b->length   = 0;
+		b->m.offset = 0;
+	}
+	DBG_MSG(" b->length:     %d\n", b->length);
+	DBG_MSG(" b->m.offset: 0x%x\n", b->m.offset);
+
+	mutex_unlock(&info->lock);
+	TRACE("Leave (index: %d)\n", b->index);
+	return 0;
+}
+
+static int cmos_cam_qbuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_drvinfo *drv = &info->drvinfo;
+	cmos_fb_t *fb;
+
+	TRACE("Enter (index: %d)\n", b->index);
+	mutex_lock(&info->lock);
+
+	fb = &drv->fb_pool[b->index];
+	put_queue(drv, fb);
+
+	mutex_unlock(&info->lock);
+	TRACE("Leave (index: %d)\n", b->index);
+	return 0;
+}
+
+static int cmos_cam_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_drvinfo *drv = &info->drvinfo;
+	unsigned long flags =0;
+	cmos_fb_t *fb = 0;
+	int ret = 0;
+
+	TRACE("Enter (index: %d)\n", b->index);
+	mutex_lock(&info->lock);
+
+	if (drv->streamoff) {
+#if 0
+		struct list_head *next;
+
+		/* CMOS sensor did not work now */
+		list_for_each(next, &drv->head) {
+			fb = (cmos_fb_t *)list_entry(next, cmos_fb_t, list);
+			if (fb) {
+				pop_queue(drv, fb);
+				break;
+			}
+		}
+#else
+		ret = -EINVAL;
+		pr_err("stream is off\n");
+#endif
+		goto EXIT_cmos_cam_dqbuf;
+	}
+
+	fb = (cmos_fb_t *)wmt_vid_get_cur_fb();
+	if (!fb) {
+		goto EXIT_cmos_cam_dqbuf;
+	}
+
+	spin_lock_irqsave(&cmos_lock, flags);
+
+	DBG_MSG("Set DQBUF on\n");
+	drv->dqbuf  = 1;
+
+	spin_unlock_irqrestore(&cmos_lock, flags);
+
+	if ((wait_event_interruptible_timeout(cmos_wait,
+					      (drv->status & STS_CMOS_FB_DONE),
+					      400) == 0)) {
+		pr_err("CMOS Time out in 400 ms\n");
+		ret = -ETIMEDOUT;
+		goto EXIT_cmos_cam_dqbuf; 
+	}
+	drv->status &= (~STS_CMOS_FB_DONE);
+
+	DBG_MSG("[%d] fb: 0x%p\n", fb->id, fb);
+	pop_queue(drv, fb);
+
+	b->length   = drv->frame_size;
+	b->m.offset = fb->y_addr;
+	b->index    = fb->id;
+
+EXIT_cmos_cam_dqbuf:
+	mutex_unlock(&info->lock);
+	TRACE("Leave (index: %d)\n", b->index);
+	return ret;
+}
+
+static int cmos_cam_enum_input(struct file *file, void *fh, struct v4l2_input *inp)
+{
+	if (inp->index >= NUM_INPUTS)
+		return -EINVAL;
+
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	inp->std = V4L2_STD_ALL;
+	sprintf(inp->name, "Camera %u", inp->index);
+	return 0;
+}
+
+static int cmos_cam_g_input(struct file *file, void *fh, unsigned int *i)
+{
+	struct cmos_info *info = file->private_data;
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+	*i = info->input;
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	return 0;
+}
+
+static int cmos_cam_s_input(struct file *file, void *fh, unsigned int i)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_dev *cd;
+	struct cmos_subdev *sd;
+	int rc,j;
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+
+	if (i >= info->input_count) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	{
+		/* Here we use info->cd[2] as nmi cmos device */
+		extern int nmi_running(void);
+		if (nmi_running()) {
+			pr_info("nmi running, use cmos device 2 ...\n");
+			i = 2;
+		}
+	}
+
+	for (j = 0; j < info->input_count; j++) {
+		if(j==i)
+			continue;
+		cd = &info->cd[j];
+		if (cd->type != CAMERA_TYPE_CMOS)
+			continue;
+		cmos_dev_power_down(cd);
+	}
+	
+	info->input = i;
+
+	cd = &info->cd[i];
+	sd = cd->sd;
+
+	cmos_dev_power_up(cd);
+
+	if (!sd) {
+		sd = cmos_subdev_get_by_try();
+		if (!sd) {
+			pr_err("cmos detect failed\n");
+			rc = -EINVAL;
+			goto err_detect;
+		}
+		pr_info("cmos #%d: detected %s\n", i, sd->name);
+		cd->sd = sd;
+	}
+
+	rc = cmos_subdev_call(sd, init);
+	if (rc) {
+		goto err_init;
+	}
+
+	pr_info("[cmos] %s init success\n", sd->name);
+	goto out;
+
+err_init:
+err_detect:
+	cmos_dev_power_down(cd);
+out:
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	return rc;
+}
+
+static int cmos_cam_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *a)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_subdev *sd = current_subdev(info);
+	int ret;
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+
+	if (a->id == V4L2_CID_CAMERA_FLASH_MODE) {
+		struct cmos_dev *cd = &info->cd[info->input];
+		ret =  (cd->cap & CAMERA_CAP_FLASH) ? v4l2_ctrl_query_fill(a, 0, 4, 1, 0) : -EINVAL;
+	} else 
+		ret = cmos_subdev_call(sd, queryctrl, a);
+
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	return ret;
+}
+
+static int cmos_cam_g_ctrl(struct file *file, void *fh, struct v4l2_control *a)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_subdev *sd = current_subdev(info);
+	int ret;
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+	ret = cmos_subdev_call(sd, g_ctrl, a);
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	return ret;
+}
+
+static int cmos_cam_s_ctrl(struct file *file, void *fh, struct v4l2_control *a)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_subdev *sd = current_subdev(info);
+	char *s;
+	int ret;
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+
+	/* for debug */
+	switch (a->id) {
+	case V4L2_CID_CAMERA_SCENE_MODE:	s = "Scene Mode";	break;
+	case V4L2_CID_DO_WHITE_BALANCE:		s = "White Balance";	break;
+	case V4L2_CID_EXPOSURE:			s = "Exposure";		break;
+	case V4L2_CID_HFLIP:			s = "H-flip";		break;
+	case V4L2_CID_VFLIP:			s = "V-flip";		break;
+	case V4L2_CID_CAMERA_FLASH_MODE:	s = "Flash Mode";	break;
+	case V4L2_CID_CAMERA_FOCUS_MODE:	s = "Focus Mode";	break;
+	case V4L2_CID_CAMERA_FOCUS_POSITION_X:	s = "Focus Area X";	break;
+	case V4L2_CID_CAMERA_FOCUS_POSITION_Y:	s = "Focus Area Y";	break;
+	default:				s = "Unknown";		break;
+	}
+	printk(KERN_DEBUG "%s: S_CTRL: %s, value %d\n", sd->name, s, a->value);
+
+	if (a->id == V4L2_CID_CAMERA_FLASH_MODE) {
+		struct cmos_dev *cd = &info->cd[info->input];
+		if (!(cd->cap & CAMERA_CAP_FLASH))
+			ret = -EINVAL;
+		else
+			ret = flash_set_mode(info->fl, a->value);
+	} else
+		ret = cmos_subdev_call(sd, s_ctrl, a);
+
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	return ret;
+}
+
+static int cmos_cam_g_parm(struct file *file, void *fh,
+		struct v4l2_streamparm *parm)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_subdev *sd = current_subdev(info);
+	int ret;
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+	ret= cmos_subdev_call(sd,g_parm, parm);
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	
+	return ret;
+}
+
+static int cmos_cam_s_parm(struct file *file, void *fh,
+		struct v4l2_streamparm *parm)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_subdev *sd = current_subdev(info);
+	int ret;
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+	ret= cmos_subdev_call(sd,s_parm, parm);
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	
+	return ret;
+}
+
+static int cmos_cam_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_drvinfo *drv = &info->drvinfo;
+	cmos_fb_t *fb;
+	int ret = 0;
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+
+	if (!list_empty(&drv->head)) {
+		fb = list_first_entry(&drv->head, cmos_fb_t, list);
+		wmt_vid_set_cur_fb((vid_fb_t *)fb);
+		cam_enable(drv, true);
+	} else
+		ret = -EINVAL;
+
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	return 0;
+}
+
+static int cmos_cam_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
+{
+	struct cmos_info *info = file->private_data;
+	struct cmos_drvinfo *drv = &info->drvinfo;
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+
+	cam_enable(drv, false);
+
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	return 0;
+}
+
+static int cmos_cam_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	int ret;
+
+	TRACE("Enter\n");
+
+	vma->vm_flags |= VM_IO | VM_RESERVED;
+	vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
+
+	ret = remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
+			      vma->vm_end - vma->vm_start, vma->vm_page_prot);
+
+	TRACE("Leave\n");
+	return ret;
+}
+
+static irqreturn_t cmos_isr(int irq, void *data)
+{
+	struct cmos_info *info = data;
+	struct cmos_drvinfo *drv;
+	struct list_head *next;
+	cmos_fb_t *cur_fb, *fb;
+	unsigned long flags = 0;
+	int mb_count = 0;
+
+	//TRACE("Enter\n");
+//	printk("i");
+
+	if (!info) {
+		return IRQ_NONE;
+	}
+
+	drv = &info->drvinfo;
+
+	if (drv->dqbuf == 1) {
+		spin_lock_irqsave(&cmos_lock, flags);
+		drv->dqbuf = 0;
+//		DBG_MSG("Set DBBUF off\n");
+		spin_unlock_irqrestore(&cmos_lock, flags);
+
+		cur_fb = (cmos_fb_t *)wmt_vid_get_cur_fb();
+		list_for_each(next, &drv->head) {
+			fb = (cmos_fb_t *) list_entry(next, cmos_fb_t, list);
+			if (fb == cur_fb) {
+				/*--------------------------------------------------------------
+				  Get next FB
+				  --------------------------------------------------------------*/
+				fb = (cmos_fb_t *) list_entry(cur_fb->list.next, cmos_fb_t, list);
+
+				do {
+					if (((vid_fb_t *)fb)->y_addr != 0)
+						mb_count = mb_counter(((vid_fb_t *)fb)->y_addr);
+
+					if (mb_count != 1) {
+						fb = (cmos_fb_t *) list_entry(fb->list.next, cmos_fb_t, list);
+//						DBG_MSG("mbcount skip 0x%08x mb_count %d \n",
+//							((vid_fb_t *)fb)->y_addr,mb_count);
+						continue;
+					}
+				} while ((mb_count != 1) && (fb != cur_fb));
+
+				cur_fb->is_busy = 0;
+				cur_fb->done    = 1;
+
+//				DBG_MSG("[%d] done: %d, is_busy: %d\n",
+//					cur_fb->id, cur_fb->done, cur_fb->is_busy);
+//				DBG_MSG("[%d] New FB done: %d, is_busy: %d\n",
+//					fb->id, fb->done, fb->is_busy);
+
+				wmt_vid_set_cur_fb((vid_fb_t *)fb);
+				drv->status |= STS_CMOS_FB_DONE;
+				wake_up_interruptible(&cmos_wait);
+				break;
+			}
+		}
+	} /* if( drv->dqbuf == 1 ) */
+
+	CMOS_REG_SET32(REG_VID_INT_CTRL, REG32_VAL(REG_VID_INT_CTRL));
+
+	//TRACE("Leave\n");
+//	printk("o");
+
+	return IRQ_HANDLED;
+}
+
+static int cmos_cam_open(struct file *filp)
+{
+	struct cmos_info *info = video_drvdata(filp);
+	struct cmos_drvinfo *drv;
+	int ret = 0;
+
+	printk(KERN_DEBUG "%s()\n", __func__);
+	//if(cam_vccvid)
+		//regulator_enable(cam_vccvid);
+	
+	//if(cam_dvdd)
+		//regulator_enable(cam_dvdd);
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+	if (cmos_dev_ref) {
+		ret = -EBUSY;
+		goto out;
+	}
+	cmos_dev_ref++;
+
+	ret = request_irq(WMT_VID_IRQ, (void *)&cmos_isr, IRQF_SHARED,
+			  "wmt-cmos", (void *)info);
+	if (ret) {
+		pr_err("CMOS: Failed to register CMOS irq %i\n", WMT_VID_IRQ);
+		cmos_dev_ref--;
+		goto out;
+	}
+
+	drv = &info->drvinfo;
+	drv->dqbuf = 0;
+	drv->status = STS_CMOS_READY;
+	INIT_LIST_HEAD(&drv->head);
+
+	spin_lock_init(&cmos_lock);
+
+	filp->private_data = info;
+
+	wmt_vid_open(VID_MODE_CMOS, NULL);
+	cam_enable(drv, false);
+
+out:
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	return ret;
+}
+
+static int cmos_cam_release(struct file *filp)
+{
+	struct cmos_info *info = filp->private_data;
+	struct cmos_drvinfo *drv = &info->drvinfo;
+	struct cmos_subdev *sd;
+	int i;
+
+	TRACE("Enter\n");
+	mutex_lock(&info->lock);
+
+	free_irq(WMT_VID_IRQ, (void *)info);
+
+	if (drv) {
+		/* Free memory */
+		for (i = 0; i < drv->fb_cnt; i++) {
+			cmos_fb_t *fb = &drv->fb_pool[i];
+			if (fb->y_addr) {
+				mb_free(fb->y_addr);
+				memset(fb, 0, sizeof(cmos_fb_t));
+			}
+		}
+	}
+
+	sd = current_subdev(info);
+	if (sd) {
+		cmos_subdev_call(sd, exit);
+	}
+	
+	cmos_dev_power_down(&info->cd[info->input]);
+	wmt_vid_close(VID_MODE_CMOS);
+	cmos_dev_ref--;
+
+//	if(cam_vccvid)
+//		regulator_disable(cam_vccvid);
+	
+	//if(cam_dvdd)
+		//regulator_disable(cam_dvdd);
+	
+	mutex_unlock(&info->lock);
+	TRACE("Leave\n");
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops cmos_ioctl_ops = {
+	.vidioc_querycap    		= cmos_cam_querycap,
+	.vidioc_enum_input		= cmos_cam_enum_input,
+	.vidioc_g_input			= cmos_cam_g_input,
+	.vidioc_s_input			= cmos_cam_s_input,
+	.vidioc_reqbufs			= cmos_cam_reqbufs,
+	.vidioc_querybuf		= cmos_cam_querybuf,
+	.vidioc_qbuf			= cmos_cam_qbuf,
+	.vidioc_dqbuf			= cmos_cam_dqbuf,
+	.vidioc_streamon		= cmos_cam_streamon,
+	.vidioc_streamoff		= cmos_cam_streamoff,
+	.vidioc_enum_fmt_vid_cap	= cmos_cam_enum_fmt_cap,
+	.vidioc_enum_framesizes		= cmos_cam_enum_framesizes,
+	.vidioc_g_fmt_vid_cap		= cmos_cam_g_fmt_cap,
+	.vidioc_s_fmt_vid_cap  		= cmos_cam_s_fmt_cap,
+	.vidioc_try_fmt_vid_cap  	= cmos_cam_try_fmt_cap,
+	.vidioc_g_parm		= cmos_cam_g_parm,
+	.vidioc_s_parm		= cmos_cam_s_parm,
+	.vidioc_queryctrl 		= cmos_cam_queryctrl,
+	.vidioc_s_ctrl       		= cmos_cam_s_ctrl,
+	.vidioc_g_ctrl       		= cmos_cam_g_ctrl,
+	.vidioc_cropcap			= cmos_cam_cropcap,
+};
+
+static const struct v4l2_file_operations cmos_cam_fops = {
+	.owner		= THIS_MODULE,
+	.open		= cmos_cam_open,
+	.release	= cmos_cam_release,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap		= cmos_cam_mmap,
+};
+
+static struct video_device cmos_cam = {
+	.name		= "cmos_cam",
+	.fops		= &cmos_cam_fops,
+	.ioctl_ops	= &cmos_ioctl_ops,
+	.release	= video_device_release_empty,
+	.tvnorms	= V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM,
+	.minor		= -1
+};
+
+static int cmos_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct cmos_info *info;
+
+	info = kzalloc(sizeof *info, GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+    // ldo6 can't disable.
+	cam_vccvid = regulator_get(NULL, "ldo6");
+	if (IS_ERR(cam_vccvid)){
+		cam_vccvid = NULL;
+		pr_err("failed to get camera vccvid\n");
+	}
+
+	cam_dvdd= regulator_get(NULL, "ldo5");
+	if (IS_ERR(cam_dvdd)){
+		cam_dvdd = NULL;
+		pr_err("failed to get camera dvdd\n");
+	}
+
+	if(cam_vccvid){
+		regulator_set_voltage(cam_vccvid, 2800000, 2800000);
+		regulator_enable(cam_vccvid);
+//		regulator_disable(cam_vccvid);
+	}
+	
+	if(cam_dvdd){
+		regulator_set_voltage(cam_dvdd, 1800000, 1800000);
+		regulator_enable(cam_dvdd);
+		//regulator_disable(cam_dvdd);
+	}
+
+	ret = parse_camera_param(info);
+	if (ret)
+		goto err_param;
+
+	ret = cmos_dev_setup(info);
+	if (ret)
+		goto err_dev_seup;
+
+	ret = video_register_device(&cmos_cam, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		pr_err("WonderMedia CMOS camera register failed\n");
+		goto err_video_register;
+	}
+
+	mutex_init(&info->lock);
+	video_set_drvdata(&cmos_cam, info);
+
+	info->fl = flash_instantiation();
+
+	pr_info("WonderMedia CMOS camera register OK \n");
+	return 0;
+
+err_video_register:
+err_dev_seup:
+err_param:
+	kfree(info);
+	return ret;
+}
+
+static int cmos_remove(struct platform_device *pdev)
+{
+	struct cmos_info *info = video_get_drvdata(&cmos_cam);
+
+	pr_info("%s()\n", __func__);
+
+	flash_destroy(info->fl);
+	video_unregister_device(&cmos_cam);
+	cmos_dev_release(info);
+	mutex_destroy(&info->lock);
+	kfree(info);
+
+	if(cam_vccvid)
+		regulator_put(cam_vccvid);
+	
+	if(cam_dvdd)
+		regulator_put(cam_dvdd);
+	return 0;
+}
+
+static int cmos_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	printk(KERN_DEBUG "%s()\n", __func__);
+	return 0;
+}
+
+static int cmos_resume(struct platform_device *pdev)
+{
+	struct cmos_info *info = video_get_drvdata(&cmos_cam);
+	struct cmos_dev *cd;
+	int i;
+
+	printk(KERN_DEBUG "%s()\n", __func__);
+
+	for (i = 0; i < info->input_count; i++) {
+		cd = &info->cd[i];
+		if (cd->type != CAMERA_TYPE_CMOS)
+			continue;
+		gpio_re_enabled(cd->pwdn_gpio);
+	}
+
+	return 0;
+}
+
+static struct platform_driver cmos_driver = {
+	.driver = {
+		.name = "cmos",
+	},
+	.remove = cmos_remove,
+	.suspend = cmos_suspend,
+	.resume = cmos_resume,
+};
+
+static void cmos_platform_release(struct device *device) { }
+
+static struct platform_device cmos_device = {
+	.name = "cmos",
+	.dev = {
+		.release = cmos_platform_release,
+	},
+};
+
+static int __init cmos_init(void)
+{
+	int ret;
+
+	cmos_subdev_init();
+
+	ret = wmt_vid_i2c_init(0, 0x52);
+	if (ret) {
+		goto err_i2c_init;
+	}
+
+	ret = platform_device_register(&cmos_device);
+	if (ret) {
+		goto err_dev_register;
+	}
+
+	ret = platform_driver_probe(&cmos_driver, cmos_probe);
+	if (ret) {
+		goto err_drv_probe;
+	}
+
+	return 0;
+
+err_drv_probe:
+	platform_device_unregister(&cmos_device);
+err_dev_register:
+	wmt_vid_i2c_release();
+err_i2c_init:
+	cmos_subdev_exit();
+	return ret;
+}
+
+static void __exit cmos_exit(void)
+{
+	pr_info("%s()\n", __func__);
+
+	platform_driver_unregister(&cmos_driver);
+	platform_device_unregister(&cmos_device);
+	wmt_vid_i2c_release();
+	cmos_subdev_exit();
+}
+
+module_init(cmos_init);
+module_exit(cmos_exit);
+
+MODULE_AUTHOR("WonderMedia SW Team Max Chen");
+MODULE_DESCRIPTION("cmos device driver");
+MODULE_LICENSE("GPL");