samsung update 1

6 files changed, 1677 insertions, 1375 deletions

diff --git a/drivers/media/video/m5mols/Kconfig b/drivers/media/video/m5mols/Kconfig
index 302dc3d..ddb3314 100644
--- a/drivers/media/video/m5mols/Kconfig
+++ b/drivers/media/video/m5mols/Kconfig
@@ -1,5 +1,6 @@
 config VIDEO_M5MOLS
 	tristate "Fujitsu M-5MOLS 8MP sensor support"
-	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API &&\
+	(VIDEO_S5P_MIPI_CSIS || VIDEO_EXYNOS_MIPI_CSIS)
 	---help---
 	  This driver supports Fujitsu M-5MOLS camera sensor with ISP
diff --git a/drivers/media/video/m5mols/Makefile b/drivers/media/video/m5mols/Makefile
index 0a44e02..b5d19bf 100644
--- a/drivers/media/video/m5mols/Makefile
+++ b/drivers/media/video/m5mols/Makefile
@@ -1,3 +1,3 @@
-m5mols-objs	:= m5mols_core.o m5mols_controls.o m5mols_capture.o
+m5mols-objs	:= m5mols_core.o m5mols_controls.o
 
 obj-$(CONFIG_VIDEO_M5MOLS)		+= m5mols.o
diff --git a/drivers/media/video/m5mols/m5mols.h b/drivers/media/video/m5mols/m5mols.h
index 89d09a8..94022e2 100644
--- a/drivers/media/video/m5mols/m5mols.h
+++ b/drivers/media/video/m5mols/m5mols.h
@@ -1,5 +1,5 @@
 /*
- * Header for M-5MOLS 8M Pixel camera sensor with ISP
+ * Header for M5MOLS 8M Pixel camera sensor with ISP
  *
  * Copyright (C) 2011 Samsung Electronics Co., Ltd.
  * Author: HeungJun Kim <riverful.kim@samsung.com>
@@ -19,12 +19,24 @@
 #include <media/v4l2-subdev.h>
 #include "m5mols_reg.h"
 
-extern int m5mols_debug;
+#define M5MO_JPEG_MAXSIZE	0x3A0000
+#define M5MO_THUMB_MAXSIZE	0xFC00
+#define M5MO_POST_MAXSIZE	0xBB800
+#define M5MO_JPEG_MEMSIZE	M5MO_JPEG_MAXSIZE + M5MO_THUMB_MAXSIZE + M5MO_POST_MAXSIZE
 
-#define to_m5mols(__sd)	container_of(__sd, struct m5mols_info, sd)
+#define v4l2msg(fmt, arg...)	do {				\
+	v4l2_dbg(1, m5mols_debug, &info->sd, fmt, ## arg);	\
+} while (0)
 
-#define to_sd(__ctrl) \
-	(&container_of(__ctrl->handler, struct m5mols_info, handle)->sd)
+extern int m5mols_debug;
+
+enum m5mols_mode {
+	MODE_SYSINIT,
+	MODE_PARMSET,
+	MODE_MONITOR,
+	MODE_CAPTURE,
+	MODE_UNKNOWN,
+};
 
 enum m5mols_restype {
 	M5MOLS_RESTYPE_MONITOR,
@@ -32,266 +44,341 @@ enum m5mols_restype {
 	M5MOLS_RESTYPE_MAX,
 };
 
-/**
- * struct m5mols_resolution - structure for the resolution
- * @type: resolution type according to the pixel code
- * @width: width of the resolution
- * @height: height of the resolution
- * @reg: resolution preset register value
- */
+enum m5mols_status {
+	STATUS_SYSINIT,
+	STATUS_PARMSET,
+	STATUS_MONITOR,
+	STATUS_AUTO_FOCUS,
+	STATUS_FACE_DETECTION,
+	STATUS_DUAL_CAPTURE,
+	STATUS_SINGLE_CAPTURE,
+	STATUS_PREVIEW,
+	STATUS_UNKNOWN,
+};
+
+enum m5mols_intterrupt_bit {
+	INT_BIT_MODE,
+	INT_BIT_AF,
+	INT_BIT_ZOOM,
+	INT_BIT_CAPTURE,
+	INT_BIT_FRAME_SYNC,
+	INT_BIT_FD,
+	INT_BIT_LENS_INIT,
+	INT_BIT_SOUND,
+};
+
+enum m5mols_i2c_size {
+	I2C_8BIT	= 1,
+	I2C_16BIT	= 2,
+	I2C_32BIT	= 4,
+	I2C_MAX		= 4,
+};
+
+enum m5mols_fps {
+	M5MOLS_FPS_AUTO	= 0,
+	M5MOLS_FPS_10	= 10,
+	M5MOLS_FPS_12	= 12,
+	M5MOLS_FPS_15	= 15,
+	M5MOLS_FPS_20	= 20,
+	M5MOLS_FPS_21	= 21,
+	M5MOLS_FPS_22	= 22,
+	M5MOLS_FPS_23	= 23,
+	M5MOLS_FPS_24	= 24,
+	M5MOLS_FPS_30	= 30,
+	M5MOLS_FPS_MAX	= M5MOLS_FPS_30,
+};
+
+enum m5mols_res_type {
+	M5MOLS_RES_MON,
+	/* It's not supported below yet. */
+	M5MOLS_RES_CAPTURE,
+	M5MOLS_RES_MAX,
+};
+
 struct m5mols_resolution {
-	u8 reg;
-	enum m5mols_restype type;
-	u16 width;
-	u16 height;
+	u8			value;
+	enum m5mols_res_type	type;
+	u16			width;
+	u16			height;
 };
 
-/**
- * struct m5mols_exif - structure for the EXIF information of M-5MOLS
- * @exposure_time: exposure time register value
- * @shutter_speed: speed of the shutter register value
- * @aperture: aperture register value
- * @exposure_bias: it calls also EV bias
- * @iso_speed: ISO register value
- * @flash: status register value of the flash
- * @sdr: status register value of the Subject Distance Range
- * @qval: not written exact meaning in document
- */
-struct m5mols_exif {
-	u32 exposure_time;
-	u32 shutter_speed;
-	u32 aperture;
-	u32 brightness;
-	u32 exposure_bias;
-	u16 iso_speed;
-	u16 flash;
-	u16 sdr;
-	u16 qval;
+struct m5mols_format {
+	enum v4l2_mbus_pixelcode code;
+	enum v4l2_colorspace colorspace;
 };
 
-/**
- * struct m5mols_capture - Structure for the capture capability
- * @exif: EXIF information
- * @main: size in bytes of the main image
- * @thumb: size in bytes of the thumb image, if it was accompanied
- * @total: total size in bytes of the produced image
- */
-struct m5mols_capture {
-	struct m5mols_exif exif;
-	u32 main;
-	u32 thumb;
-	u32 total;
+struct m5mols_control {
+	u32	id;
+	s32	min;
+	s32	max;
+	u32	step;
+	s32	def;
 };
 
-/**
- * struct m5mols_scenemode - structure for the scenemode capability
- * @metering: metering light register value
- * @ev_bias: EV bias register value
- * @wb_mode: mode which means the WhiteBalance is Auto or Manual
- * @wb_preset: whitebalance preset register value in the Manual mode
- * @chroma_en: register value whether the Chroma capability is enabled or not
- * @chroma_lvl: chroma's level register value
- * @edge_en: register value Whether the Edge capability is enabled or not
- * @edge_lvl: edge's level register value
- * @af_range: Auto Focus's range
- * @fd_mode: Face Detection mode
- * @mcc: Multi-axis Color Conversion which means emotion color
- * @light: status of the Light
- * @flash: status of the Flash
- * @tone: Tone color which means Contrast
- * @iso: ISO register value
- * @capt_mode: Mode of the Image Stabilization while the camera capturing
- * @wdr: Wide Dynamic Range register value
- *
- * The each value according to each scenemode is recommended in the documents.
- */
-struct m5mols_scenemode {
-	u8 metering;
-	u8 ev_bias;
-	u8 wb_mode;
-	u8 wb_preset;
-	u8 chroma_en;
-	u8 chroma_lvl;
-	u8 edge_en;
-	u8 edge_lvl;
-	u8 af_range;
-	u8 fd_mode;
-	u8 mcc;
-	u8 light;
-	u8 flash;
-	u8 tone;
-	u8 iso;
-	u8 capt_mode;
-	u8 wdr;
+struct m5mols_exif {
+	u32	exposure_time;
+	u32	shutter_speed;
+	u32	aperture;
+	u32	brightness;
+	u32	exposure_bias;
+	u16	iso_speed;
+	u16	flash;
+	u16	sdr;		/* subject(object) distance range */
+	u16	qval;		/* This is not written precisely in datasheet. */
+};
+
+struct m5mols_capture {
+	struct m5mols_exif		exif;
+	u32				main;
+	u32				thumb;
+	u32				total;
 };
 
-/**
- * struct m5mols_version - firmware version information
- * @customer:	customer information
- * @project:	version of project information according to customer
- * @fw:		firmware revision
- * @hw:		hardware revision
- * @param:	version of the parameter
- * @awb:	Auto WhiteBalance algorithm version
- * @str:	information about manufacturer and packaging vendor
- * @af:		Auto Focus version
- *
- * The register offset starts the customer version at 0x0, and it ends
- * the awb version at 0x09. The customer, project information occupies 1 bytes
- * each. And also the fw, hw, param, awb each requires 2 bytes. The str is
- * unique string associated with firmware's version. It includes information
- * about manufacturer and the vendor of the sensor's packaging. The least
- * significant 2 bytes of the string indicate packaging manufacturer.
- */
-#define VERSION_STRING_SIZE	22
 struct m5mols_version {
-	u8	customer;
-	u8	project;
-	u16	fw;
-	u16	hw;
-	u16	param;
-	u16	awb;
-	u8	str[VERSION_STRING_SIZE];
-	u8	af;
+	u8	ctm_code;	/* customer code */
+	u8	pj_code;	/* project code */
+	u16	fw;		/* firmware version */
+	u16	hw;		/* hardware version */
+	u16	parm;		/* parameter version */
+	u16	awb;		/* AWB version */
 };
 
-/**
- * struct m5mols_info - M-5MOLS driver data structure
- * @pdata: platform data
- * @sd: v4l-subdev instance
- * @pad: media pad
- * @ffmt: current fmt according to resolution type
- * @res_type: current resolution type
- * @code: current code
- * @irq_waitq: waitqueue for the capture
- * @work_irq: workqueue for the IRQ
- * @flags: state variable for the interrupt handler
- * @handle: control handler
- * @autoexposure: Auto Exposure control
- * @exposure: Exposure control
- * @autowb: Auto White Balance control
- * @colorfx: Color effect control
- * @saturation:	Saturation control
- * @zoom: Zoom control
- * @ver: information of the version
- * @cap: the capture mode attributes
- * @power: current sensor's power status
- * @ctrl_sync: true means all controls of the sensor are initialized
- * @int_capture: true means the capture interrupt is issued once
- * @lock_ae: true means the Auto Exposure is locked
- * @lock_awb: true means the Aut WhiteBalance is locked
- * @resolution:	register value for current resolution
- * @interrupt: register value for current interrupt status
- * @mode: register value for current operation mode
- * @mode_save: register value for current operation mode for saving
- * @set_power: optional power callback to the board code
- */
 struct m5mols_info {
-	const struct m5mols_platform_data *pdata;
-	struct v4l2_subdev sd;
+	struct v4l2_subdev		sd;
 	struct media_pad pad;
-	struct v4l2_mbus_framefmt ffmt[M5MOLS_RESTYPE_MAX];
 	int res_type;
-	enum v4l2_mbus_pixelcode code;
-	wait_queue_head_t irq_waitq;
-	struct work_struct work_irq;
-	unsigned long flags;
+	u8 resolution;
+	struct v4l2_mbus_framefmt	fmt[M5MOLS_RES_MAX];
+	struct v4l2_fract		tpf;
 
-	struct v4l2_ctrl_handler handle;
-	/* Autoexposure/exposure control cluster */
+	struct v4l2_ctrl_handler	handle;
 	struct {
-		struct v4l2_ctrl *autoexposure;
-		struct v4l2_ctrl *exposure;
+		/* support only AE of the Monitor Mode in this version */
+		struct v4l2_ctrl	*autoexposure;
+		struct v4l2_ctrl	*exposure;
+		bool			is_ae_lock;
 	};
-	struct v4l2_ctrl *autowb;
-	struct v4l2_ctrl *colorfx;
-	struct v4l2_ctrl *saturation;
-	struct v4l2_ctrl *zoom;
-
-	struct m5mols_version ver;
-	struct m5mols_capture cap;
-	bool power;
-	bool ctrl_sync;
-	bool lock_ae;
-	bool lock_awb;
-	u8 resolution;
-	u8 interrupt;
-	u8 mode;
-	u8 mode_save;
+	struct v4l2_ctrl		*autofocus;
+	bool				is_focus;
+	struct v4l2_ctrl		*autowb;
+	bool				is_awb_lock;
+	struct v4l2_ctrl		*colorfx;
+	struct v4l2_ctrl		*saturation;
+	struct v4l2_ctrl		*zoom;
+	struct v4l2_ctrl		*jpeg_size;
+	struct v4l2_ctrl		*encoded_size;
+
+	enum m5mols_mode		mode;
+	enum m5mols_mode		mode_backup;
+	enum m5mols_status		status;
+	enum v4l2_mbus_pixelcode	code;
+
+	struct m5mols_capture		cap;
+	wait_queue_head_t		cap_wait;
+	bool				captured;
+
+	const struct m5mols_platform_data	*pdata;
+	struct m5mols_version		ver;
+	struct work_struct		work;
+	bool				power;
+
+	/* for additional power if needed. */
 	int (*set_power)(struct device *dev, int on);
 };
 
-#define ST_CAPT_IRQ 0
+/* control functions */
+int m5mols_set_ctrl(struct v4l2_ctrl *ctrl);
 
-#define is_powered(__info) (__info->power)
-#define is_ctrl_synced(__info) (__info->ctrl_sync)
-#define is_available_af(__info)	(__info->ver.af)
-#define is_code(__code, __type) (__code == m5mols_default_ffmt[__type].code)
-#define is_manufacturer(__info, __manufacturer)	\
-				(__info->ver.str[0] == __manufacturer[0] && \
-				 __info->ver.str[1] == __manufacturer[1])
-/*
- * I2C operation of the M-5MOLS
- *
- * The I2C read operation of the M-5MOLS requires 2 messages. The first
- * message sends the information about the command, command category, and total
- * message size. The second message is used to retrieve the data specifed in
- * the first message
- *
- *   1st message                                2nd message
- *   +-------+---+----------+-----+-------+     +------+------+------+------+
- *   | size1 | R | category | cmd | size2 |     | d[0] | d[1] | d[2] | d[3] |
- *   +-------+---+----------+-----+-------+     +------+------+------+------+
- *   - size1: message data size(5 in this case)
- *   - size2: desired buffer size of the 2nd message
- *   - d[0..3]: according to size2
- *
- * The I2C write operation needs just one message. The message includes
- * category, command, total size, and desired data.
- *
- *   1st message
- *   +-------+---+----------+-----+------+------+------+------+
- *   | size1 | W | category | cmd | d[0] | d[1] | d[2] | d[3] |
- *   +-------+---+----------+-----+------+------+------+------+
- *   - d[0..3]: according to size1
- */
-int m5mols_read_u8(struct v4l2_subdev *sd, u32 reg_comb, u8 *val);
-int m5mols_read_u16(struct v4l2_subdev *sd, u32 reg_comb, u16 *val);
-int m5mols_read_u32(struct v4l2_subdev *sd, u32 reg_comb, u32 *val);
-int m5mols_write(struct v4l2_subdev *sd, u32 reg_comb, u32 val);
-int m5mols_busy(struct v4l2_subdev *sd, u8 category, u8 cmd, u8 value);
+/* I2C functions - referenced by below I2C helper functions */
+int m5mols_read_reg(struct v4l2_subdev *sd, enum m5mols_i2c_size size,
+		u8 category, u8 cmd, u32 *val);
+int m5mols_write_reg(struct v4l2_subdev *sd, enum m5mols_i2c_size size,
+		u8 category, u8 cmd, u32 val);
+int m5mols_check_busy(struct v4l2_subdev *sd,
+		u8 category, u8 cmd, u32 value);
+int m5mols_set_mode(struct v4l2_subdev *sd, enum m5mols_mode mode);
+enum m5mols_status m5mols_get_status(struct v4l2_subdev *sd);
 
 /*
- * Mode operation of the M-5MOLS
- *
- * Changing the mode of the M-5MOLS is needed right executing order.
- * There are three modes(PARAMETER, MONITOR, CAPTURE) which can be changed
- * by user. There are various categories associated with each mode.
- *
- * +============================================================+
- * | mode	| category					|
- * +============================================================+
- * | FLASH	| FLASH(only after Stand-by or Power-on)	|
- * | SYSTEM	| SYSTEM(only after sensor arm-booting)		|
- * | PARAMETER	| PARAMETER					|
- * | MONITOR	| MONITOR(preview), Auto Focus, Face Detection	|
- * | CAPTURE	| Single CAPTURE, Preview(recording)		|
- * +============================================================+
- *
- * The available executing order between each modes are as follows:
- *   PARAMETER <---> MONITOR <---> CAPTURE
+ * helper functions
  */
-int m5mols_mode(struct m5mols_info *info, u8 mode);
+static inline struct m5mols_info *to_m5mols(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct m5mols_info, sd);
+}
 
-int m5mols_enable_interrupt(struct v4l2_subdev *sd, u8 reg);
-int m5mols_sync_controls(struct m5mols_info *info);
-int m5mols_start_capture(struct m5mols_info *info);
-int m5mols_do_scenemode(struct m5mols_info *info, u8 mode);
-int m5mols_lock_3a(struct m5mols_info *info, bool lock);
-int m5mols_set_ctrl(struct v4l2_ctrl *ctrl);
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct m5mols_info, handle)->sd;
+}
+
+static inline bool is_streaming(struct v4l2_subdev *sd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	return (info->mode == MODE_MONITOR) || (info->mode == MODE_CAPTURE);
+}
+
+static inline bool is_stoped(struct v4l2_subdev *sd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	return (info->mode != MODE_MONITOR) && (info->mode != MODE_CAPTURE);
+}
+
+static inline bool is_powerup(struct v4l2_subdev *sd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	return info->power;
+}
+
+static inline int m5mols_set_mode_backup(struct v4l2_subdev *sd,
+		enum m5mols_mode mode)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+
+	info->mode_backup = info->mode;
+	return m5mols_set_mode(sd, mode);
+}
+
+static inline int m5mols_set_mode_restore(struct v4l2_subdev *sd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	int ret;
+
+	ret = m5mols_set_mode(sd, info->mode_backup);
+	if (!ret)
+		info->mode = info->mode_backup;
+	return ret;
+}
+
+static inline int __must_check i2c_w8_system(struct v4l2_subdev *sd,
+		u8 cmd, u32 val)
+{
+	return m5mols_write_reg(sd, I2C_8BIT, CAT_SYSTEM, cmd, val);
+}
+
+static inline int __must_check i2c_w8_param(struct v4l2_subdev *sd,
+		u8 cmd, u32 val)
+{
+	return m5mols_write_reg(sd, I2C_8BIT, CAT_PARAM, cmd, val);
+}
+
+static inline int __must_check i2c_w8_mon(struct v4l2_subdev *sd,
+		u8 cmd, u32 val)
+{
+	return m5mols_write_reg(sd, I2C_8BIT, CAT_MON, cmd, val);
+}
+
+static inline int __must_check i2c_w8_ae(struct v4l2_subdev *sd,
+		u8 cmd, u32 val)
+{
+	return m5mols_write_reg(sd, I2C_8BIT, CAT_AE, cmd, val);
+}
+
+static inline int __must_check i2c_w16_ae(struct v4l2_subdev *sd,
+		u8 cmd, u32 val)
+{
+	return m5mols_write_reg(sd, I2C_16BIT, CAT_AE, cmd, val);
+}
+
+static inline int __must_check i2c_w8_wb(struct v4l2_subdev *sd,
+		u8 cmd, u32 val)
+{
+	return m5mols_write_reg(sd, I2C_8BIT, CAT_WB, cmd, val);
+}
+
+static inline int __must_check i2c_w8_lens(struct v4l2_subdev *sd,
+		u8 cmd, u32 val)
+{
+	return m5mols_write_reg(sd, I2C_8BIT, CAT_LENS, cmd, val);
+}
+
+static inline int __must_check i2c_w8_capt_parm(struct v4l2_subdev *sd,
+		u8 cmd, u32 val)
+{
+	return m5mols_write_reg(sd, I2C_8BIT, CAT_CAPTURE_PARAMETER, cmd, val);
+}
+
+static inline int __must_check i2c_w8_capt_ctrl(struct v4l2_subdev *sd,
+		u8 cmd, u32 val)
+{
+	return m5mols_write_reg(sd, I2C_8BIT, CAT_CAPTURE_CONTROL, cmd, val);
+}
+
+static inline int __must_check i2c_w8_flash(struct v4l2_subdev *sd,
+		u8 cmd, u32 val)
+{
+	return m5mols_write_reg(sd, I2C_8BIT, CAT_FLASH, cmd, val);
+}
+
+static inline int __must_check i2c_r8_system(struct v4l2_subdev *sd,
+		u8 cmd, u32 *val)
+{
+	return m5mols_read_reg(sd, I2C_8BIT, CAT_SYSTEM, cmd, val);
+}
+
+static inline int __must_check i2c_r8_param(struct v4l2_subdev *sd,
+		u8 cmd, u32 *val)
+{
+	return m5mols_read_reg(sd, I2C_8BIT, CAT_PARAM, cmd, val);
+}
+
+static inline int __must_check i2c_r8_mon(struct v4l2_subdev *sd,
+		u8 cmd, u32 *val)
+{
+	return m5mols_read_reg(sd, I2C_8BIT, CAT_MON, cmd, val);
+}
+
+static inline int __must_check i2c_r8_ae(struct v4l2_subdev *sd,
+		u8 cmd, u32 *val)
+{
+	return m5mols_read_reg(sd, I2C_8BIT, CAT_AE, cmd, val);
+}
+
+static inline int __must_check i2c_r16_ae(struct v4l2_subdev *sd,
+		u8 cmd, u32 *val)
+{
+	return m5mols_read_reg(sd, I2C_16BIT, CAT_AE, cmd, val);
+}
+
+static inline int __must_check i2c_r8_lens(struct v4l2_subdev *sd,
+		u8 cmd, u32 *val)
+{
+	return m5mols_read_reg(sd, I2C_8BIT, CAT_LENS, cmd, val);
+}
+
+static inline int __must_check i2c_r32_capt_ctrl(struct v4l2_subdev *sd,
+		u8 cmd, u32 *val)
+{
+	return m5mols_read_reg(sd, I2C_32BIT, CAT_CAPTURE_CONTROL, cmd, val);
+}
+
+static inline int __must_check i2c_r16_exif(struct v4l2_subdev *sd,
+		u8 cmd, u32 *val)
+{
+	return m5mols_read_reg(sd, I2C_16BIT, CAT_EXIF, cmd, val);
+}
+
+static inline int __must_check i2c_r32_exif(struct v4l2_subdev *sd,
+		u8 cmd, u32 *val)
+{
+	return m5mols_read_reg(sd, I2C_32BIT, CAT_EXIF, cmd, val);
+}
+
+static int m5mols_set_ae_lock(struct m5mols_info *info, bool lock)
+{
+	struct v4l2_subdev *sd = &info->sd;
+
+	info->is_ae_lock = lock;
+
+	return i2c_w8_ae(sd, CAT3_AE_LOCK, !!lock);
+}
+
+static int m5mols_set_awb_lock(struct m5mols_info *info, bool lock)
+{
+	struct v4l2_subdev *sd = &info->sd;
 
-/* The firmware function */
-int m5mols_update_fw(struct v4l2_subdev *sd,
-		     int (*set_power)(struct m5mols_info *, bool));
+	info->is_awb_lock = lock;
 
+	return i2c_w8_wb(sd, CAT6_AWB_LOCK, !!lock);
+}
 #endif	/* M5MOLS_H */
diff --git a/drivers/media/video/m5mols/m5mols_controls.c b/drivers/media/video/m5mols/m5mols_controls.c
index d135d20..3e08018 100644
--- a/drivers/media/video/m5mols/m5mols_controls.c
+++ b/drivers/media/video/m5mols/m5mols_controls.c
@@ -1,5 +1,5 @@
 /*
- * Controls for M-5MOLS 8M Pixel camera sensor with ISP
+ * Controls for M5MOLS 8M Pixel camera sensor with ISP
  *
  * Copyright (C) 2011 Samsung Electronics Co., Ltd.
  * Author: HeungJun Kim <riverful.kim@samsung.com>
@@ -14,285 +14,186 @@
  */
 
 #include <linux/i2c.h>
-#include <linux/delay.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-ctrls.h>
 
 #include "m5mols.h"
 #include "m5mols_reg.h"
 
-static struct m5mols_scenemode m5mols_default_scenemode[] = {
-	[REG_SCENE_NORMAL] = {
-		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
-		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
-		REG_AF_NORMAL, REG_FD_OFF,
-		REG_MCC_NORMAL, REG_LIGHT_OFF, REG_FLASH_OFF,
-		5, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
-	},
-	[REG_SCENE_PORTRAIT] = {
-		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
-		REG_CHROMA_ON, 3, REG_EDGE_ON, 4,
-		REG_AF_NORMAL, BIT_FD_EN | BIT_FD_DRAW_FACE_FRAME,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
-	},
-	[REG_SCENE_LANDSCAPE] = {
-		REG_AE_ALL, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
-		REG_CHROMA_ON, 4, REG_EDGE_ON, 6,
-		REG_AF_NORMAL, REG_FD_OFF,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
-	},
-	[REG_SCENE_SPORTS] = {
-		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
-		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
-		REG_AF_NORMAL, REG_FD_OFF,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
-	},
-	[REG_SCENE_PARTY_INDOOR] = {
-		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
-		REG_CHROMA_ON, 4, REG_EDGE_ON, 5,
-		REG_AF_NORMAL, REG_FD_OFF,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_200, REG_CAP_NONE, REG_WDR_OFF,
-	},
-	[REG_SCENE_BEACH_SNOW] = {
-		REG_AE_CENTER, REG_AE_INDEX_10_POS, REG_AWB_AUTO, 0,
-		REG_CHROMA_ON, 4, REG_EDGE_ON, 5,
-		REG_AF_NORMAL, REG_FD_OFF,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_50, REG_CAP_NONE, REG_WDR_OFF,
-	},
-	[REG_SCENE_SUNSET] = {
-		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_PRESET,
-		REG_AWB_DAYLIGHT,
-		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
-		REG_AF_NORMAL, REG_FD_OFF,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
-	},
-	[REG_SCENE_DAWN_DUSK] = {
-		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_PRESET,
-		REG_AWB_FLUORESCENT_1,
-		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
-		REG_AF_NORMAL, REG_FD_OFF,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
-	},
-	[REG_SCENE_FALL] = {
-		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
-		REG_CHROMA_ON, 5, REG_EDGE_ON, 5,
-		REG_AF_NORMAL, REG_FD_OFF,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
-	},
-	[REG_SCENE_NIGHT] = {
-		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
-		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
-		REG_AF_NORMAL, REG_FD_OFF,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
-	},
-	[REG_SCENE_AGAINST_LIGHT] = {
-		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
-		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
-		REG_AF_NORMAL, REG_FD_OFF,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
-	},
-	[REG_SCENE_FIRE] = {
-		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
-		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
-		REG_AF_NORMAL, REG_FD_OFF,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_50, REG_CAP_NONE, REG_WDR_OFF,
-	},
-	[REG_SCENE_TEXT] = {
-		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
-		REG_CHROMA_ON, 3, REG_EDGE_ON, 7,
-		REG_AF_MACRO, REG_FD_OFF,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_AUTO, REG_CAP_ANTI_SHAKE, REG_WDR_ON,
-	},
-	[REG_SCENE_CANDLE] = {
-		REG_AE_CENTER, REG_AE_INDEX_00, REG_AWB_AUTO, 0,
-		REG_CHROMA_ON, 3, REG_EDGE_ON, 5,
-		REG_AF_NORMAL, REG_FD_OFF,
-		REG_MCC_OFF, REG_LIGHT_OFF, REG_FLASH_OFF,
-		6, REG_ISO_AUTO, REG_CAP_NONE, REG_WDR_OFF,
-	},
-};
+static int m5mols_wb_mode(struct m5mols_info *info, struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	int ret;
 
-/**
- * m5mols_do_scenemode() - Change current scenemode
- * @mode:	Desired mode of the scenemode
- *
- * WARNING: The execution order is important. Do not change the order.
- */
-int m5mols_do_scenemode(struct m5mols_info *info, u8 mode)
+	if (info->is_awb_lock) {
+		ret = m5mols_set_awb_lock(info, false);
+		if (!ret)
+			return ret;
+	}
+
+	/* 0x01 : Auto Whitebalance, 0x02 : Manual Whitebalance. */
+	return i2c_w8_wb(sd, CAT6_AWB_MODE, (ctrl->val) ? 0x1 : 0x2);
+
+}
+
+static int m5mols_exposure_mode(struct m5mols_info *info,
+		struct v4l2_ctrl *ctrl)
 {
 	struct v4l2_subdev *sd = &info->sd;
-	struct m5mols_scenemode scenemode = m5mols_default_scenemode[mode];
 	int ret;
 
-	if (mode > REG_SCENE_CANDLE)
-		return -EINVAL;
+	if (info->is_ae_lock) {
+		ret = m5mols_set_ae_lock(info, false);
+		if (ret)
+			return ret;
+	}
 
-	ret = m5mols_lock_3a(info, false);
-	if (!ret)
-		ret = m5mols_write(sd, AE_EV_PRESET_MONITOR, mode);
-	if (!ret)
-		ret = m5mols_write(sd, AE_EV_PRESET_CAPTURE, mode);
-	if (!ret)
-		ret = m5mols_write(sd, AE_MODE, scenemode.metering);
-	if (!ret)
-		ret = m5mols_write(sd, AE_INDEX, scenemode.ev_bias);
-	if (!ret)
-		ret = m5mols_write(sd, AWB_MODE, scenemode.wb_mode);
-	if (!ret)
-		ret = m5mols_write(sd, AWB_MANUAL, scenemode.wb_preset);
-	if (!ret)
-		ret = m5mols_write(sd, MON_CHROMA_EN, scenemode.chroma_en);
-	if (!ret)
-		ret = m5mols_write(sd, MON_CHROMA_LVL, scenemode.chroma_lvl);
-	if (!ret)
-		ret = m5mols_write(sd, MON_EDGE_EN, scenemode.edge_en);
-	if (!ret)
-		ret = m5mols_write(sd, MON_EDGE_LVL, scenemode.edge_lvl);
-	if (!ret && is_available_af(info))
-		ret = m5mols_write(sd, AF_MODE, scenemode.af_range);
-	if (!ret && is_available_af(info))
-		ret = m5mols_write(sd, FD_CTL, scenemode.fd_mode);
-	if (!ret)
-		ret = m5mols_write(sd, MON_TONE_CTL, scenemode.tone);
-	if (!ret)
-		ret = m5mols_write(sd, AE_ISO, scenemode.iso);
-	if (!ret)
-		ret = m5mols_mode(info, REG_CAPTURE);
-	if (!ret)
-		ret = m5mols_write(sd, CAPP_WDR_EN, scenemode.wdr);
-	if (!ret)
-		ret = m5mols_write(sd, CAPP_MCC_MODE, scenemode.mcc);
-	if (!ret)
-		ret = m5mols_write(sd, CAPP_LIGHT_CTRL, scenemode.light);
+	/* 0x01 : Auto Exposure, 0x0 : Manual Exposure. */
+	return i2c_w8_ae(sd, CAT3_AE_MODE,
+			(ctrl->val == V4L2_EXPOSURE_AUTO) ? 0x1 : 0x0);
+}
+
+static int m5mols_exposure(struct m5mols_info *info)
+{
+	struct v4l2_subdev *sd = &info->sd;
+
+	return i2c_w16_ae(sd, CAT3_MANUAL_GAIN_MON, info->exposure->val);
+}
+
+static int m5mols_zoom(struct m5mols_info *info, struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = &info->sd;
+
+	return i2c_w8_mon(sd, CAT2_ZOOM, ctrl->val);
+}
+
+static int m5mols_focus_mode(struct m5mols_info *info,
+		struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = &info->sd;
+	u32 reg;
+	int ret;
+
+	ret = m5mols_set_mode(sd, MODE_MONITOR);
 	if (!ret)
-		ret = m5mols_write(sd, CAPP_FLASH_CTRL, scenemode.flash);
+		ret = i2c_r8_system(sd, CAT0_INT_FACTOR, &reg);
 	if (!ret)
-		ret = m5mols_write(sd, CAPC_MODE, scenemode.capt_mode);
+		ret = i2c_w8_system(sd, CAT0_INT_ENABLE, (1 << INT_BIT_AF));
 	if (!ret)
-		ret = m5mols_mode(info, REG_MONITOR);
+		/* must be excuted in the monitor mode */
+		ret = i2c_w8_lens(sd, CATA_INIT_AF_FUNC, 0x1);
 
-	return ret;
+	/* 0x0 : Normal AF mode
+	 * 0x1 : Macro AF mode
+	 * 0x2 : Continuous AF mode (Not working) */
+	return i2c_w8_lens(sd, CATA_AF_MODE, 0x0);
 }
 
-static int m5mols_lock_ae(struct m5mols_info *info, bool lock)
+static int m5mols_focus(struct m5mols_info *info, struct v4l2_ctrl *ctrl)
 {
-	int ret = 0;
-
-	if (info->lock_ae != lock)
-		ret = m5mols_write(&info->sd, AE_LOCK,
-				lock ? REG_AE_LOCK : REG_AE_UNLOCK);
-	if (!ret)
-		info->lock_ae = lock;
+	struct v4l2_subdev *sd = &info->sd;
 
-	return ret;
+	/* 0x0: Stop,
+	   0x1: Excute AF,
+	   0x02: Excutre MF rel(Not tested),
+	   0x03: Excute MF absol(Not tested) */
+	return i2c_w8_lens(sd, CATA_AF_EXCUTE, ctrl->val);
 }
 
-static int m5mols_lock_awb(struct m5mols_info *info, bool lock)
+static int m5mols_set_saturation(struct m5mols_info *info,
+		struct v4l2_ctrl *ctrl)
 {
-	int ret = 0;
+	struct v4l2_subdev *sd = &info->sd;
+	static u8 m5mols_chroma_lvl[] = {
+		0x1c, 0x3e, 0x5f, 0x80, 0xa1, 0xc2, 0xe4,
+	};
+	int ret;
 
-	if (info->lock_awb != lock)
-		ret = m5mols_write(&info->sd, AWB_LOCK,
-				lock ? REG_AWB_LOCK : REG_AWB_UNLOCK);
+	ret = i2c_w8_mon(sd, CAT2_CHROMA_LVL, m5mols_chroma_lvl[ctrl->val]);
 	if (!ret)
-		info->lock_awb = lock;
+		ret = i2c_w8_mon(sd, CAT2_CHROMA_EN, true);
 
 	return ret;
 }
 
-/* m5mols_lock_3a() - Lock 3A(Auto Exposure, Auto Whitebalance, Auto Focus) */
-int m5mols_lock_3a(struct m5mols_info *info, bool lock)
+static int m5mols_set_colorfx(struct m5mols_info *info, struct v4l2_ctrl *ctrl)
 {
-	int ret;
-
-	ret = m5mols_lock_ae(info, lock);
-	if (!ret)
-		ret = m5mols_lock_awb(info, lock);
-	/* Don't need to handle unlocking AF */
-	if (!ret && is_available_af(info) && lock)
-		ret = m5mols_write(&info->sd, AF_EXECUTE, REG_AF_STOP);
+	struct v4l2_subdev *sd = &info->sd;
+	static u8 m5mols_effects_gamma[] = {	/* cat 1: Effects */
+		[V4L2_COLORFX_NEGATIVE]		= 0x01,
+		[V4L2_COLORFX_EMBOSS]		= 0x06,
+		[V4L2_COLORFX_SKETCH]		= 0x07,
+	};
+	static u8 m5mols_cfixb_chroma[] = {	/* cat 2: Cr for effect */
+		[V4L2_COLORFX_BW]		= 0x0,
+		[V4L2_COLORFX_SEPIA]		= 0xd8,
+		[V4L2_COLORFX_SKY_BLUE]		= 0x40,
+		[V4L2_COLORFX_GRASS_GREEN]	= 0xe0,
+	};
+	static u8 m5mols_cfixr_chroma[] = {	/* cat 2: Cb for effect */
+		[V4L2_COLORFX_BW]		= 0x0,
+		[V4L2_COLORFX_SEPIA]		= 0x18,
+		[V4L2_COLORFX_SKY_BLUE]		= 0x00,
+		[V4L2_COLORFX_GRASS_GREEN]	= 0xe0,
+	};
+	int ret = -EINVAL;
+
+	switch (ctrl->val) {
+	case V4L2_COLORFX_NONE:
+		return i2c_w8_mon(sd, CAT2_COLOR_EFFECT, false);
+	case V4L2_COLORFX_BW:		/* chroma: Gray */
+	case V4L2_COLORFX_SEPIA:	/* chroma: Sepia */
+	case V4L2_COLORFX_SKY_BLUE:	/* chroma: Blue */
+	case V4L2_COLORFX_GRASS_GREEN:	/* chroma: Green */
+		ret = i2c_w8_mon(sd, CAT2_CFIXB,
+				m5mols_cfixb_chroma[ctrl->val]);
+		if (!ret)
+			ret = i2c_w8_mon(sd, CAT2_CFIXR,
+					m5mols_cfixr_chroma[ctrl->val]);
+		if (!ret)
+			ret = i2c_w8_mon(sd, CAT2_COLOR_EFFECT, true);
+		return ret;
+	case V4L2_COLORFX_NEGATIVE:	/* gamma: Negative */
+	case V4L2_COLORFX_EMBOSS:	/* gamma: Emboss */
+	case V4L2_COLORFX_SKETCH:	/* gamma: Outline */
+		ret = i2c_w8_param(sd, CAT1_EFFECT,
+				m5mols_effects_gamma[ctrl->val]);
+		if (!ret)
+			ret = i2c_w8_mon(sd, CAT2_COLOR_EFFECT, true);
+		return ret;
+	}
 
 	return ret;
 }
 
-/* m5mols_set_ctrl() - The main s_ctrl function called by m5mols_set_ctrl() */
 int m5mols_set_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct v4l2_subdev *sd = to_sd(ctrl);
 	struct m5mols_info *info = to_m5mols(sd);
-	int ret;
+	int ret = 0;
 
 	switch (ctrl->id) {
 	case V4L2_CID_ZOOM_ABSOLUTE:
-		return m5mols_write(sd, MON_ZOOM, ctrl->val);
-
+		return m5mols_zoom(info, ctrl);
+	case V4L2_CID_FOCUS_AUTO:
+		if (ctrl->val != 0)
+			ret = m5mols_focus_mode(info, ctrl);
+		if (!ret)
+			ret = m5mols_focus(info, ctrl);
+		return ret;
 	case V4L2_CID_EXPOSURE_AUTO:
-		ret = m5mols_lock_ae(info,
-			ctrl->val == V4L2_EXPOSURE_AUTO ? false : true);
-		if (!ret && ctrl->val == V4L2_EXPOSURE_AUTO)
-			ret = m5mols_write(sd, AE_MODE, REG_AE_ALL);
-		if (!ret && ctrl->val == V4L2_EXPOSURE_MANUAL) {
-			int val = info->exposure->val;
-			ret = m5mols_write(sd, AE_MODE, REG_AE_OFF);
-			if (!ret)
-				ret = m5mols_write(sd, AE_MAN_GAIN_MON, val);
-			if (!ret)
-				ret = m5mols_write(sd, AE_MAN_GAIN_CAP, val);
-		}
+		if (!ctrl->is_new)
+			ctrl->val = V4L2_EXPOSURE_MANUAL;
+		ret = m5mols_exposure_mode(info, ctrl);
+		if (!ret && ctrl->val == V4L2_EXPOSURE_MANUAL)
+			ret = m5mols_exposure(info);
 		return ret;
-
 	case V4L2_CID_AUTO_WHITE_BALANCE:
-		ret = m5mols_lock_awb(info, ctrl->val ? false : true);
-		if (!ret)
-			ret = m5mols_write(sd, AWB_MODE, ctrl->val ?
-				REG_AWB_AUTO : REG_AWB_PRESET);
-		return ret;
-
+		return m5mols_wb_mode(info, ctrl);
 	case V4L2_CID_SATURATION:
-		ret = m5mols_write(sd, MON_CHROMA_LVL, ctrl->val);
-		if (!ret)
-			ret = m5mols_write(sd, MON_CHROMA_EN, REG_CHROMA_ON);
-		return ret;
-
+		return m5mols_set_saturation(info, ctrl);
 	case V4L2_CID_COLORFX:
-		/*
-		 * This control uses two kinds of registers: normal & color.
-		 * The normal effect belongs to category 1, while the color
-		 * one belongs to category 2.
-		 *
-		 * The normal effect uses one register: CAT1_EFFECT.
-		 * The color effect uses three registers:
-		 * CAT2_COLOR_EFFECT, CAT2_CFIXR, CAT2_CFIXB.
-		 */
-		ret = m5mols_write(sd, PARM_EFFECT,
-			ctrl->val == V4L2_COLORFX_NEGATIVE ? REG_EFFECT_NEGA :
-			ctrl->val == V4L2_COLORFX_EMBOSS ? REG_EFFECT_EMBOSS :
-			REG_EFFECT_OFF);
-		if (!ret)
-			ret = m5mols_write(sd, MON_EFFECT,
-				ctrl->val == V4L2_COLORFX_SEPIA ?
-				REG_COLOR_EFFECT_ON : REG_COLOR_EFFECT_OFF);
-		if (!ret)
-			ret = m5mols_write(sd, MON_CFIXR,
-				ctrl->val == V4L2_COLORFX_SEPIA ?
-				REG_CFIXR_SEPIA : 0);
-		if (!ret)
-			ret = m5mols_write(sd, MON_CFIXB,
-				ctrl->val == V4L2_COLORFX_SEPIA ?
-				REG_CFIXB_SEPIA : 0);
-		return ret;
+		return m5mols_set_colorfx(info, ctrl);
 	}
 
 	return -EINVAL;
diff --git a/drivers/media/video/m5mols/m5mols_core.c b/drivers/media/video/m5mols/m5mols_core.c
index 43c68f5..cf4c8ba 100644
--- a/drivers/media/video/m5mols/m5mols_core.c
+++ b/drivers/media/video/m5mols/m5mols_core.c
@@ -1,5 +1,5 @@
 /*
- * Driver for M-5MOLS 8M Pixel camera sensor with ISP
+ * Driver for M5MOLS 8M Pixel camera sensor with ISP
  *
  * Copyright (C) 2011 Samsung Electronics Co., Ltd.
  * Author: HeungJun Kim <riverful.kim@samsung.com>
@@ -22,6 +22,7 @@
 #include <linux/gpio.h>
 #include <linux/regulator/consumer.h>
 #include <linux/videodev2.h>
+#include <linux/videodev2_exynos_media.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
@@ -33,35 +34,66 @@
 int m5mols_debug;
 module_param(m5mols_debug, int, 0644);
 
-#define MODULE_NAME		"M5MOLS"
-#define M5MOLS_I2C_CHECK_RETRY	500
+#define MOD_NAME		"M5MOLS"
+#define M5MOLS_I2C_CHECK_RETRY 50
+#define DEBUG
+#define DEFAULT_SENSOR_WIDTH	800
+#define DEFAULT_SENSOR_HEIGHT	480
+
+#define m5_err	\
+	do { printk("%s : %d : ret : %d\n", __func__, __LINE__, ret);\
+	} while(0)
+/* M5MOLS mode */
+static u8 m5mols_reg_mode[] = {
+	[MODE_SYSINIT]		= 0x00,
+	[MODE_PARMSET]		= 0x01,
+	[MODE_MONITOR]		= 0x02,
+	[MODE_CAPTURE]		= 0x03,
+	[MODE_UNKNOWN]		= 0xff,
+};
+
+/* M5MOLS status */
+static u8 m5mols_reg_status[] = {
+	[STATUS_SYSINIT]	= 0x00,
+	[STATUS_PARMSET]	= 0x01,
+	[STATUS_MONITOR]	= 0x02,
+	[STATUS_AUTO_FOCUS]	= 0x03,
+	[STATUS_FACE_DETECTION]	= 0x04,
+	[STATUS_DUAL_CAPTURE]	= 0x05,
+	[STATUS_SINGLE_CAPTURE]	= 0x06,
+	[STATUS_PREVIEW]	= 0x07,
+	[STATUS_UNKNOWN]	= 0xff,
+};
 
-/* The regulator consumer names for external voltage regulators */
+/* M5MOLS regulator consumer names */
+/* The DEFAULT names of power are referenced with M5MO datasheet. */
 static struct regulator_bulk_data supplies[] = {
 	{
-		.supply = "core",	/* ARM core power, 1.2V */
+		/* core power - 1.2v, generally at the M5MOLS */
+		.supply		= "core",
 	}, {
-		.supply	= "dig_18",	/* digital power 1, 1.8V */
+		.supply		= "dig_18",	/* digital power 1 - 1.8v */
 	}, {
-		.supply	= "d_sensor",	/* sensor power 1, 1.8V */
+		.supply		= "d_sensor",	/* sensor power 1 - 1.8v */
 	}, {
-		.supply	= "dig_28",	/* digital power 2, 2.8V */
+		.supply		= "dig_28",	/* digital power 2 - 2.8v */
 	}, {
-		.supply	= "a_sensor",	/* analog power */
+		.supply		= "a_sensor",	/* analog power */
 	}, {
-		.supply	= "dig_12",	/* digital power 3, 1.2V */
+		.supply		= "dig_12",	/* digital power 3 - 1.2v */
 	},
 };
 
-static struct v4l2_mbus_framefmt m5mols_default_ffmt[M5MOLS_RESTYPE_MAX] = {
-	[M5MOLS_RESTYPE_MONITOR] = {
-		.width		= 1920,
-		.height		= 1080,
-		.code		= V4L2_MBUS_FMT_VYUY8_2X8,
+/* M5MOLS default format (codes, sizes, preset values) */
+static struct v4l2_mbus_framefmt default_fmt[M5MOLS_RES_MAX] = {
+	[M5MOLS_RES_MON] = {
+		.width		= DEFAULT_SENSOR_WIDTH,
+		.height		= DEFAULT_SENSOR_HEIGHT,
+		.code		= V4L2_MBUS_FMT_YUYV8_2X8,
 		.field		= V4L2_FIELD_NONE,
 		.colorspace	= V4L2_COLORSPACE_JPEG,
 	},
-	[M5MOLS_RESTYPE_CAPTURE] = {
+	[M5MOLS_RES_CAPTURE] = {
 		.width		= 1920,
 		.height		= 1080,
 		.code		= V4L2_MBUS_FMT_JPEG_1X8,
@@ -69,189 +101,201 @@ static struct v4l2_mbus_framefmt m5mols_default_ffmt[M5MOLS_RESTYPE_MAX] = {
 		.colorspace	= V4L2_COLORSPACE_JPEG,
 	},
 };
-#define SIZE_DEFAULT_FFMT	ARRAY_SIZE(m5mols_default_ffmt)
-
-static const struct m5mols_resolution m5mols_reg_res[] = {
-	{ 0x01, M5MOLS_RESTYPE_MONITOR, 128, 96 },	/* SUB-QCIF */
-	{ 0x03, M5MOLS_RESTYPE_MONITOR, 160, 120 },	/* QQVGA */
-	{ 0x05, M5MOLS_RESTYPE_MONITOR, 176, 144 },	/* QCIF */
-	{ 0x06, M5MOLS_RESTYPE_MONITOR, 176, 176 },
-	{ 0x08, M5MOLS_RESTYPE_MONITOR, 240, 320 },	/* QVGA */
-	{ 0x09, M5MOLS_RESTYPE_MONITOR, 320, 240 },	/* QVGA */
-	{ 0x0c, M5MOLS_RESTYPE_MONITOR, 240, 400 },	/* WQVGA */
-	{ 0x0d, M5MOLS_RESTYPE_MONITOR, 400, 240 },	/* WQVGA */
-	{ 0x0e, M5MOLS_RESTYPE_MONITOR, 352, 288 },	/* CIF */
-	{ 0x13, M5MOLS_RESTYPE_MONITOR, 480, 360 },
-	{ 0x15, M5MOLS_RESTYPE_MONITOR, 640, 360 },	/* qHD */
-	{ 0x17, M5MOLS_RESTYPE_MONITOR, 640, 480 },	/* VGA */
-	{ 0x18, M5MOLS_RESTYPE_MONITOR, 720, 480 },
-	{ 0x1a, M5MOLS_RESTYPE_MONITOR, 800, 480 },	/* WVGA */
-	{ 0x1f, M5MOLS_RESTYPE_MONITOR, 800, 600 },	/* SVGA */
-	{ 0x21, M5MOLS_RESTYPE_MONITOR, 1280, 720 },	/* HD */
-	{ 0x25, M5MOLS_RESTYPE_MONITOR, 1920, 1080 },	/* 1080p */
-	{ 0x29, M5MOLS_RESTYPE_MONITOR, 3264, 2448 },	/* 2.63fps 8M */
-	{ 0x39, M5MOLS_RESTYPE_MONITOR, 800, 602 },	/* AHS_MON debug */
-
-	{ 0x02, M5MOLS_RESTYPE_CAPTURE, 320, 240 },	/* QVGA */
-	{ 0x04, M5MOLS_RESTYPE_CAPTURE, 400, 240 },	/* WQVGA */
-	{ 0x07, M5MOLS_RESTYPE_CAPTURE, 480, 360 },
-	{ 0x08, M5MOLS_RESTYPE_CAPTURE, 640, 360 },	/* qHD */
-	{ 0x09, M5MOLS_RESTYPE_CAPTURE, 640, 480 },	/* VGA */
-	{ 0x0a, M5MOLS_RESTYPE_CAPTURE, 800, 480 },	/* WVGA */
-	{ 0x10, M5MOLS_RESTYPE_CAPTURE, 1280, 720 },	/* HD */
-	{ 0x14, M5MOLS_RESTYPE_CAPTURE, 1280, 960 },	/* 1M */
-	{ 0x17, M5MOLS_RESTYPE_CAPTURE, 1600, 1200 },	/* 2M */
-	{ 0x19, M5MOLS_RESTYPE_CAPTURE, 1920, 1080 },	/* Full-HD */
-	{ 0x1a, M5MOLS_RESTYPE_CAPTURE, 2048, 1152 },	/* 3Mega */
-	{ 0x1b, M5MOLS_RESTYPE_CAPTURE, 2048, 1536 },
-	{ 0x1c, M5MOLS_RESTYPE_CAPTURE, 2560, 1440 },	/* 4Mega */
-	{ 0x1d, M5MOLS_RESTYPE_CAPTURE, 2560, 1536 },
-	{ 0x1f, M5MOLS_RESTYPE_CAPTURE, 2560, 1920 },	/* 5Mega */
-	{ 0x21, M5MOLS_RESTYPE_CAPTURE, 3264, 1836 },	/* 6Mega */
-	{ 0x22, M5MOLS_RESTYPE_CAPTURE, 3264, 1960 },
-	{ 0x25, M5MOLS_RESTYPE_CAPTURE, 3264, 2448 },	/* 8Mega */
+
+#define SIZE_DEFAULT_FFMT	ARRAY_SIZE(default_fmt)
+
+static const struct m5mols_format m5mols_formats[] = {
+	[M5MOLS_RES_MON] = {
+		.code		= V4L2_MBUS_FMT_YUYV8_2X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+	},
+	[M5MOLS_RES_CAPTURE] = {
+		.code		= V4L2_MBUS_FMT_JPEG_1X8,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+	},
+};
+
+static const struct m5mols_resolution m5mols_resolutions[] = {
+	/* monitor size */
+	{ 0x01, M5MOLS_RES_MON, 128, 96 },	/* SUB-QCIF */
+	{ 0x03, M5MOLS_RES_MON, 160, 120 },	/* QQVGA */
+	{ 0x05, M5MOLS_RES_MON, 176, 144 },	/* QCIF */
+	{ 0x06, M5MOLS_RES_MON, 176, 176 },	/* 176*176 */
+	{ 0x08, M5MOLS_RES_MON, 240, 320 },	/* 1 QVGA */
+	{ 0x09, M5MOLS_RES_MON, 320, 240 },	/* QVGA */
+	{ 0x0c, M5MOLS_RES_MON, 240, 400 },	/* l WQVGA */
+	{ 0x0d, M5MOLS_RES_MON, 400, 240 },	/* WQVGA */
+	{ 0x0e, M5MOLS_RES_MON, 352, 288 },	/* CIF */
+	{ 0x13, M5MOLS_RES_MON, 480, 360 },	/* 480*360 */
+	{ 0x15, M5MOLS_RES_MON, 640, 360 },	/* qHD */
+	{ 0x17, M5MOLS_RES_MON, 640, 480 },	/* VGA */
+	{ 0x18, M5MOLS_RES_MON, 720, 480 },	/* 720x480 */
+	{ 0x1a, M5MOLS_RES_MON, 800, 480 },	/* WVGA */
+	{ 0x1f, M5MOLS_RES_MON, 800, 600 },	/* SVGA */
+	{ 0x21, M5MOLS_RES_MON, 1280, 720 },	/* HD */
+	{ 0x25, M5MOLS_RES_MON, 1920, 1080 },	/* 1080p */
+	{ 0x29, M5MOLS_RES_MON, 3264, 2448 },	/* 8M (2.63fps@3264*2448) */
+	{ 0x30, M5MOLS_RES_MON, 320, 240 },	/* 60fps for slow motion */
+	{ 0x31, M5MOLS_RES_MON, 320, 240 },	/* 120fps for slow motion */
+	{ 0x39, M5MOLS_RES_MON, 800, 602 },	/* AHS_MON debug */
+
+	/* capture(JPEG or Bayer RAW or YUV Raw) size */
+	{ 0x02, M5MOLS_RES_CAPTURE, 320, 240 },		/* QVGA */
+	{ 0x04, M5MOLS_RES_CAPTURE, 400, 240 },		/* WQVGA */
+	{ 0x07, M5MOLS_RES_CAPTURE, 480, 360 },		/* 480 x 360 */
+	{ 0x08, M5MOLS_RES_CAPTURE, 640, 360 },		/* qHD */
+	{ 0x09, M5MOLS_RES_CAPTURE, 640, 480 },		/* VGA */
+	{ 0x0a, M5MOLS_RES_CAPTURE, 800, 480 },		/* WVGA */
+	{ 0x10, M5MOLS_RES_CAPTURE, 1280, 720 },	/* HD */
+	{ 0x14, M5MOLS_RES_CAPTURE, 1280,  960 },	/* 1M */
+	{ 0x17, M5MOLS_RES_CAPTURE, 1600, 1200 },	/* 2M */
+	{ 0x19, M5MOLS_RES_CAPTURE, 1920, 1080 },	/* Full-HD */
+	{ 0x1a, M5MOLS_RES_CAPTURE, 2048, 1152 },	/* 3M */
+	{ 0x1b, M5MOLS_RES_CAPTURE, 2048, 1536 },	/* 3M */
+	{ 0x1c, M5MOLS_RES_CAPTURE, 2560, 1440 },	/* 4M */
+	{ 0x1d, M5MOLS_RES_CAPTURE, 2560, 1536 },	/* 4M */
+	{ 0x1f, M5MOLS_RES_CAPTURE, 2560, 1920 },	/* 5M */
+	{ 0x21, M5MOLS_RES_CAPTURE, 3264, 1836 },	/* 6M */
+	{ 0x22, M5MOLS_RES_CAPTURE, 3264, 1960 },	/* 6M */
+	{ 0x25, M5MOLS_RES_CAPTURE, 3264, 2448 },	/* 8M */
+#ifdef M5MO_THUMB_SUPPORT
+	/* capture thumb(JPEG) size */
+	{ 0x00, M5MOLS_RES_THUMB, 160, 90 },	/* 160 x 90 */
+	{ 0x02, M5MOLS_RES_THUMB, 160, 120 },	/* QQVGA */
+	{ 0x04, M5MOLS_RES_THUMB, 320, 240 },	/* QVGA */
+	{ 0x06, M5MOLS_RES_THUMB, 400, 240 },	/* WQVGA */
+	{ 0x09, M5MOLS_RES_THUMB, 480, 360 },	/* 480 x 360 */
+	{ 0x0a, M5MOLS_RES_THUMB, 640, 360 },	/* qHD */
+	{ 0x0b, M5MOLS_RES_THUMB, 640, 480 },	/* VGA */
+	{ 0x0c, M5MOLS_RES_THUMB, 800, 480 },	/* WVGA */
+#endif
 };
 
-/**
- * m5mols_swap_byte - an byte array to integer conversion function
- * @size: size in bytes of I2C packet defined in the M-5MOLS datasheet
- *
- * Convert I2C data byte array with performing any required byte
- * reordering to assure proper values for each data type, regardless
- * of the architecture endianness.
- */
-static u32 m5mols_swap_byte(u8 *data, u8 length)
+/* M5MOLS default FPS */
+static const struct v4l2_fract default_fps = {
+	.numerator		= 1,
+	.denominator		= M5MOLS_FPS_AUTO,
+};
+
+static u8 m5mols_reg_fps[] = {
+	[M5MOLS_FPS_AUTO]	= 0x01,
+	[M5MOLS_FPS_10]		= 0x05,
+	[M5MOLS_FPS_12]		= 0x04,
+	[M5MOLS_FPS_15]		= 0x03,
+	[M5MOLS_FPS_20]		= 0x08,
+	[M5MOLS_FPS_21]		= 0x09,
+	[M5MOLS_FPS_22]		= 0x0a,
+	[M5MOLS_FPS_23]		= 0x0b,
+	[M5MOLS_FPS_24]		= 0x07,
+	[M5MOLS_FPS_30]		= 0x02,
+};
+
+static u32 m5mols_swap_byte(u8 *data, enum m5mols_i2c_size size)
 {
-	if (length == 1)
+	if (size == I2C_8BIT)
 		return *data;
-	else if (length == 2)
+	else if (size == I2C_16BIT)
 		return be16_to_cpu(*((u16 *)data));
 	else
 		return be32_to_cpu(*((u32 *)data));
 }
 
-/**
- * m5mols_read -  I2C read function
- * @reg: combination of size, category and command for the I2C packet
- * @size: desired size of I2C packet
- * @val: read value
+/*
+ * m5mols_read_reg/m5mols_write_reg - handle sensor's I2C communications.
+ *
+ * The I2C command packet of M5MOLS is made up 3 kinds of I2C bytes(category,
+ * command, bytes). Reference m5mols.h.
+ *
+ * The packet is needed 2, when M5MOLS is read through I2C.
+ * The packet is needed 1, when M5MOLS is written through I2C.
+ *
+ * I2C packet common order(including both reading/writing)
+ *   1st : size (data size + 4)
+ *   2nd : READ/WRITE (R - 0x01, W - 0x02)
+ *   3rd : Category
+ *   4th : Command
+ *
+ * I2C packet order for READING operation
+ *   5th : data real size for reading
+ *   And, read another I2C packet again, until data size.
+ *
+ * I2C packet order for WRITING operation
+ *   5th to 8th: an actual data to write
  */
-static int m5mols_read(struct v4l2_subdev *sd, u32 size, u32 reg, u32 *val)
+
+#define M5MOLS_BYTE_READ	0x01
+#define M5MOLS_BYTE_WRITE	0x02
+
+int m5mols_read_reg(struct v4l2_subdev *sd,
+		enum m5mols_i2c_size size,
+		u8 category, u8 cmd, u32 *val)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	u8 rbuf[M5MOLS_I2C_MAX_SIZE + 1];
-	u8 category = I2C_CATEGORY(reg);
-	u8 cmd = I2C_COMMAND(reg);
 	struct i2c_msg msg[2];
-	u8 wbuf[5];
+	u8 wbuf[5], rbuf[I2C_MAX + 1];
 	int ret;
 
 	if (!client->adapter)
 		return -ENODEV;
 
+	if (size != I2C_8BIT && size != I2C_16BIT && size != I2C_32BIT)
+		return -EINVAL;
+
+	/* 1st I2C operation for writing category & command. */
 	msg[0].addr = client->addr;
 	msg[0].flags = 0;
-	msg[0].len = 5;
+	msg[0].len = 5;		/* 1(cmd size per bytes) + 4 */
 	msg[0].buf = wbuf;
-	wbuf[0] = 5;
+	wbuf[0] = 5;		/* same right above this */
 	wbuf[1] = M5MOLS_BYTE_READ;
 	wbuf[2] = category;
 	wbuf[3] = cmd;
 	wbuf[4] = size;
 
+	/* 2nd I2C operation for reading data. */
 	msg[1].addr = client->addr;
 	msg[1].flags = I2C_M_RD;
 	msg[1].len = size + 1;
 	msg[1].buf = rbuf;
 
-	/* minimum stabilization time */
-	usleep_range(200, 200);
-
 	ret = i2c_transfer(client->adapter, msg, 2);
 	if (ret < 0) {
-		v4l2_err(sd, "read failed: size:%d cat:%02x cmd:%02x. %d\n",
-			 size, category, cmd, ret);
+		m5_err;
+		dev_err(&client->dev, "failed READ[%d] at "
+				"cat[%02x] cmd[%02x]\n",
+				size, category, cmd);
 		return ret;
 	}
 
 	*val = m5mols_swap_byte(&rbuf[1], size);
 
-	return 0;
-}
-
-int m5mols_read_u8(struct v4l2_subdev *sd, u32 reg, u8 *val)
-{
-	u32 val_32;
-	int ret;
+	usleep_range(15000, 20000);	/* must be for stabilization */
 
-	if (I2C_SIZE(reg) != 1) {
-		v4l2_err(sd, "Wrong data size\n");
-		return -EINVAL;
-	}
-
-	ret = m5mols_read(sd, I2C_SIZE(reg), reg, &val_32);
-	if (ret)
-		return ret;
-
-	*val = (u8)val_32;
-	return ret;
-}
-
-int m5mols_read_u16(struct v4l2_subdev *sd, u32 reg, u16 *val)
-{
-	u32 val_32;
-	int ret;
-
-	if (I2C_SIZE(reg) != 2) {
-		v4l2_err(sd, "Wrong data size\n");
-		return -EINVAL;
-	}
-
-	ret = m5mols_read(sd, I2C_SIZE(reg), reg, &val_32);
-	if (ret)
-		return ret;
-
-	*val = (u16)val_32;
-	return ret;
-}
-
-int m5mols_read_u32(struct v4l2_subdev *sd, u32 reg, u32 *val)
-{
-	if (I2C_SIZE(reg) != 4) {
-		v4l2_err(sd, "Wrong data size\n");
-		return -EINVAL;
-	}
-
-	return m5mols_read(sd, I2C_SIZE(reg), reg, val);
+	return 0;
 }
 
-/**
- * m5mols_write - I2C command write function
- * @reg: combination of size, category and command for the I2C packet
- * @val: value to write
- */
-int m5mols_write(struct v4l2_subdev *sd, u32 reg, u32 val)
+int m5mols_write_reg(struct v4l2_subdev *sd,
+		enum m5mols_i2c_size size,
+		u8 category, u8 cmd, u32 val)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	u8 wbuf[M5MOLS_I2C_MAX_SIZE + 4];
-	u8 category = I2C_CATEGORY(reg);
-	u8 cmd = I2C_COMMAND(reg);
-	u8 size	= I2C_SIZE(reg);
-	u32 *buf = (u32 *)&wbuf[4];
+	struct device *cdev = &client->dev;
 	struct i2c_msg msg[1];
+	u8 wbuf[I2C_MAX + 4];
+	u32 *buf = (u32 *)&wbuf[4];
 	int ret;
 
 	if (!client->adapter)
 		return -ENODEV;
 
-	if (size != 1 && size != 2 && size != 4) {
-		v4l2_err(sd, "Wrong data size\n");
+	if (size != I2C_8BIT && size != I2C_16BIT && size != I2C_32BIT) {
+		dev_err(cdev, "Wrong data size\n");
 		return -EINVAL;
 	}
 
 	msg->addr = client->addr;
 	msg->flags = 0;
-	msg->len = (u16)size + 4;
+	msg->len = size + 4;
 	msg->buf = wbuf;
 	wbuf[0] = size + 4;
 	wbuf[1] = M5MOLS_BYTE_WRITE;
@@ -260,551 +304,925 @@ int m5mols_write(struct v4l2_subdev *sd, u32 reg, u32 val)
 
 	*buf = m5mols_swap_byte((u8 *)&val, size);
 
-	usleep_range(200, 200);
-
 	ret = i2c_transfer(client->adapter, msg, 1);
 	if (ret < 0) {
-		v4l2_err(sd, "write failed: size:%d cat:%02x cmd:%02x. %d\n",
-			size, category, cmd, ret);
+		m5_err;
+		dev_err(&client->dev, "failed WRITE[%d] at "
+				"cat[%02x] cmd[%02x], ret %d\n",
+				size, msg->buf[2], msg->buf[3], ret);
 		return ret;
 	}
 
+	usleep_range(15000, 20000);	/* must be for stabilization */
+
 	return 0;
 }
 
-int m5mols_busy(struct v4l2_subdev *sd, u8 category, u8 cmd, u8 mask)
+int m5mols_check_busy(struct v4l2_subdev *sd, u8 category, u8 cmd, u32 value)
 {
-	u8 busy;
-	int i;
+	u32 busy, i;
 	int ret;
 
 	for (i = 0; i < M5MOLS_I2C_CHECK_RETRY; i++) {
-		ret = m5mols_read_u8(sd, I2C_REG(category, cmd, 1), &busy);
+		ret = m5mols_read_reg(sd, I2C_8BIT, category, cmd, &busy);
 		if (ret < 0)
 			return ret;
-		if ((busy & mask) == mask)
+
+		if (busy == value)	/* bingo */
 			return 0;
+
+		/* must be for stabilization */
+		usleep_range(10000, 10000);
 	}
 	return -EBUSY;
 }
 
-/**
- * m5mols_enable_interrupt - Clear interrupt pending bits and unmask interrupts
+/*
+ * m5mols_set_mode - change and set mode of M5MOLS.
  *
- * Before writing desired interrupt value the INT_FACTOR register should
- * be read to clear pending interrupts.
+ * This driver supports now only 3 modes(System, Monitor, Parameter).
  */
-int m5mols_enable_interrupt(struct v4l2_subdev *sd, u8 reg)
+int m5mols_set_mode(struct v4l2_subdev *sd, enum m5mols_mode mode)
 {
 	struct m5mols_info *info = to_m5mols(sd);
-	u8 mask = is_available_af(info) ? REG_INT_AF : 0;
-	u8 dummy;
-	int ret;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *cdev = &client->dev;
+	const char *m5mols_str_mode[] = {
+		"System initialization",
+		"Parameter setting",
+		"Monitor setting",
+		"Capture setting",
+		"Unknown",
+	};
+	int ret = 0;
+
+	if (mode < MODE_SYSINIT || mode > MODE_UNKNOWN)
+		return -EINVAL;
+
+	ret = i2c_w8_system(sd, CAT0_SYSMODE, m5mols_reg_mode[mode]);
+	if (!ret) {
+		/* bug detect, capture status is  not 0x3 but 0x6 */
+		if (mode == MODE_CAPTURE)
+			mode = STATUS_SINGLE_CAPTURE;
+		ret = m5mols_check_busy(sd, CAT_SYSTEM, CAT0_STATUS,
+				m5mols_reg_status[mode]);
+		if (ret)
+			m5_err;
+	}
+	if (ret < 0)
+		return ret;
+
+	info->mode = m5mols_reg_mode[mode];
+	dev_dbg(cdev, " mode: %s\n", m5mols_str_mode[mode]);
 
-	ret = m5mols_read_u8(sd, SYSTEM_INT_FACTOR, &dummy);
-	if (!ret)
-		ret = m5mols_write(sd, SYSTEM_INT_ENABLE, reg & ~mask);
 	return ret;
 }
 
-/**
- * m5mols_reg_mode - Write the mode and check busy status
- *
- * It always accompanies a little delay changing the M-5MOLS mode, so it is
- * needed checking current busy status to guarantee right mode.
+/*
+ * m5mols_get_status - get status of M5MOLS.
  */
-static int m5mols_reg_mode(struct v4l2_subdev *sd, u8 mode)
+enum m5mols_status m5mols_get_status(struct v4l2_subdev *sd)
 {
-	int ret = m5mols_write(sd, SYSTEM_SYSMODE, mode);
+	struct m5mols_info *info = to_m5mols(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *cdev = &client->dev;
+	const char *m5mols_str_status[] = {
+		"System initialization",
+		"Parameter setting",
+		"Monitor setting",
+		"Auto Focus",
+		"Face Detection",
+		"Multi/Dual Capture",
+		"Single Capture",
+		"Preview (Data transfer)", /* It means recording, not preview. */
+		"Unknown",
+	};
+	u32 reg;
+	int ret = 0;
+
+	ret = i2c_r8_system(sd, CAT0_STATUS, &reg);
+	if (ret)
+		return ret;
+
+	if (reg < STATUS_SYSINIT || reg >= STATUS_UNKNOWN)
+		return -EINVAL;
 
-	return ret ? ret : m5mols_busy(sd, CAT_SYSTEM, CAT0_SYSMODE, mode);
+	info->status = m5mols_reg_status[reg];
+	dev_dbg(cdev, " status: %s\n", m5mols_str_status[reg]);
+
+	return ret;
 }
 
-/**
- * m5mols_mode - manage the M-5MOLS's mode
- * @mode: the required operation mode
- *
- * The commands of M-5MOLS are grouped into specific modes. Each functionality
- * can be guaranteed only when the sensor is operating in mode which which
- * a command belongs to.
+/*
+ * get_version - get M5MOLS sensor versions.
  */
-int m5mols_mode(struct m5mols_info *info, u8 mode)
+static int get_version(struct v4l2_subdev *sd)
 {
-	struct v4l2_subdev *sd = &info->sd;
-	int ret = -EINVAL;
-	u8 reg;
+	struct m5mols_info *info = to_m5mols(sd);
+	union {
+		struct m5mols_version	ver;
+		u8			bytes[10];
+	} value;
+	int ret, i;
+
+	for (i = CAT0_CUSTOMER_CODE; i <= CAT0_VERSION_AWB_L; i++) {
+		ret = i2c_r8_system(sd, i, (u32 *)&value.bytes[i]);
+		if (ret)
+			return ret;
+	}
 
-	if (mode < REG_PARAMETER && mode > REG_CAPTURE)
-		return ret;
+	info->ver = value.ver;
 
-	ret = m5mols_read_u8(sd, SYSTEM_SYSMODE, &reg);
-	if ((!ret && reg == mode) || ret)
-		return ret;
+	info->ver.fw = be16_to_cpu(info->ver.fw);
+	info->ver.hw = be16_to_cpu(info->ver.hw);
+	info->ver.parm = be16_to_cpu(info->ver.parm);
+	info->ver.awb = be16_to_cpu(info->ver.awb);
 
-	switch (reg) {
-	case REG_PARAMETER:
-		ret = m5mols_reg_mode(sd, REG_MONITOR);
-		if (!ret && mode == REG_MONITOR)
-			break;
-		if (!ret)
-			ret = m5mols_reg_mode(sd, REG_CAPTURE);
-		break;
+	return ret;
+}
 
-	case REG_MONITOR:
-		if (mode == REG_PARAMETER) {
-			ret = m5mols_reg_mode(sd, REG_PARAMETER);
-			break;
-		}
+static void m5mols_show_version(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	struct m5mols_info *info = to_m5mols(sd);
 
-		ret = m5mols_reg_mode(sd, REG_CAPTURE);
-		break;
+	dev_info(dev, "customer code\t0x%02x\n", info->ver.ctm_code);
+	dev_info(dev, "project code\t0x%02x\n", info->ver.pj_code);
+	dev_info(dev, "firmware version\t0x%04x\n", info->ver.fw);
+	dev_info(dev, "hardware version\t0x%04x\n", info->ver.hw);
+	dev_info(dev, "parameter version\t0x%04x\n", info->ver.parm);
+	dev_info(dev, "AWB version\t0x%04x\n", info->ver.awb);
+}
 
-	case REG_CAPTURE:
-		ret = m5mols_reg_mode(sd, REG_MONITOR);
-		if (!ret && mode == REG_MONITOR)
-			break;
-		if (!ret)
-			ret = m5mols_reg_mode(sd, REG_PARAMETER);
-		break;
+/*
+ * get_res_preset - find out M5MOLS register value from requested resolution.
+ *
+ * @width: requested width
+ * @height: requested height
+ * @type: requested type of each modes. It supports only monitor mode now.
+ */
+static int get_res_preset(struct v4l2_subdev *sd, u16 width, u16 height,
+		enum m5mols_res_type type)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	int i;
 
-	default:
-		v4l2_warn(sd, "Wrong mode: %d\n", mode);
+	for (i = 0; i < ARRAY_SIZE(m5mols_resolutions); i++) {
+		if ((m5mols_resolutions[i].type == type) &&
+			(m5mols_resolutions[i].width == width) &&
+			(m5mols_resolutions[i].height == height))
+			break;
 	}
 
-	if (!ret)
-		info->mode = mode;
+	if (i >= ARRAY_SIZE(m5mols_resolutions)) {
+		v4l2msg("no matching resolution\n");
+		return -EINVAL;
+	}
 
-	return ret;
+	return m5mols_resolutions[i].value;
 }
 
-/**
- * m5mols_get_version - retrieve full revisions information of M-5MOLS
+/*
+ * get_fps - calc & check FPS from v4l2_captureparm, if FPS is adequate, set.
  *
- * The version information includes revisions of hardware and firmware,
- * AutoFocus alghorithm version and the version string.
+ * In M5MOLS case, the denominator means FPS. The each value of numerator and
+ * denominator should not be minus. If numerator is 0, it sets AUTO FPS. If
+ * numerator is not 1, it recalculates denominator. After it checks, the
+ * denominator is set to timeperframe.denominator, and used by FPS.
  */
-static int m5mols_get_version(struct v4l2_subdev *sd)
+static int get_fps(struct v4l2_subdev *sd,
+		struct v4l2_captureparm *parm)
 {
-	struct m5mols_info *info = to_m5mols(sd);
-	struct m5mols_version *ver = &info->ver;
-	u8 *str = ver->str;
-	int i;
-	int ret;
+	int numerator = parm->timeperframe.numerator;
+	int denominator = parm->timeperframe.denominator;
 
-	ret = m5mols_read_u8(sd, SYSTEM_VER_CUSTOMER, &ver->customer);
-	if (!ret)
-		ret = m5mols_read_u8(sd, SYSTEM_VER_PROJECT, &ver->project);
-	if (!ret)
-		ret = m5mols_read_u16(sd, SYSTEM_VER_FIRMWARE, &ver->fw);
-	if (!ret)
-		ret = m5mols_read_u16(sd, SYSTEM_VER_HARDWARE, &ver->hw);
-	if (!ret)
-		ret = m5mols_read_u16(sd, SYSTEM_VER_PARAMETER, &ver->param);
-	if (!ret)
-		ret = m5mols_read_u16(sd, SYSTEM_VER_AWB, &ver->awb);
-	if (!ret)
-		ret = m5mols_read_u8(sd, AF_VERSION, &ver->af);
-	if (ret)
-		return ret;
+	/* The denominator should be +, except 0. The numerator shoud be +. */
+	if (numerator < 0 || denominator <= 0)
+		return -EINVAL;
 
-	for (i = 0; i < VERSION_STRING_SIZE; i++) {
-		ret = m5mols_read_u8(sd, SYSTEM_VER_STRING, &str[i]);
-		if (ret)
-			return ret;
+	/* The numerator is 0, return auto fps. */
+	if (numerator == 0) {
+		parm->timeperframe.denominator = M5MOLS_FPS_AUTO;
+		return 0;
 	}
 
-	ver->fw = be16_to_cpu(ver->fw);
-	ver->hw = be16_to_cpu(ver->hw);
-	ver->param = be16_to_cpu(ver->param);
-	ver->awb = be16_to_cpu(ver->awb);
+	/* calc FPS(not time per frame) per 1 numerator */
+	denominator = denominator / numerator;
 
-	v4l2_info(sd, "Manufacturer\t[%s]\n",
-			is_manufacturer(info, REG_SAMSUNG_ELECTRO) ?
-			"Samsung Electro-Machanics" :
-			is_manufacturer(info, REG_SAMSUNG_OPTICS) ?
-			"Samsung Fiber-Optics" :
-			is_manufacturer(info, REG_SAMSUNG_TECHWIN) ?
-			"Samsung Techwin" : "None");
-	v4l2_info(sd, "Customer/Project\t[0x%02x/0x%02x]\n",
-			info->ver.customer, info->ver.project);
+	if (denominator < M5MOLS_FPS_AUTO || denominator > M5MOLS_FPS_MAX)
+		return -EINVAL;
 
-	if (!is_available_af(info))
-		v4l2_info(sd, "No support Auto Focus on this firmware\n");
+	if (!m5mols_reg_fps[denominator])
+		return -EINVAL;
 
-	return ret;
+	return 0;
 }
 
-/**
- * __find_restype - Lookup M-5MOLS resolution type according to pixel code
- * @code: pixel code
+/*
+ * to_code - return pixelcode of M5MOLS according to resolution type.
  */
-static enum m5mols_restype __find_restype(enum v4l2_mbus_pixelcode code)
+static enum v4l2_mbus_pixelcode to_code(enum m5mols_res_type res_type)
 {
-	enum m5mols_restype type = M5MOLS_RESTYPE_MONITOR;
+	return m5mols_formats[res_type].code;
+}
 
-	do {
-		if (code == m5mols_default_ffmt[type].code)
-			return type;
-	} while (type++ != SIZE_DEFAULT_FFMT);
+/*
+ * to_res_type - return resolution type of M5MOLS according to pixelcode.
+ */
+static enum m5mols_res_type to_res_type(struct v4l2_subdev *sd,
+		enum v4l2_mbus_pixelcode code)
+{
+	int i = ARRAY_SIZE(m5mols_formats);
 
-	return 0;
+	while (i--)
+		if (code == m5mols_formats[i].code)
+			break;
+	if (i < 0)
+		return M5MOLS_RES_MAX;
+
+	if (code == m5mols_formats[M5MOLS_RES_MON].code)
+		return M5MOLS_RES_MON;
+	else
+		return M5MOLS_RES_CAPTURE;
 }
 
-/**
- * __find_resolution - Lookup preset and type of M-5MOLS's resolution
- * @mf: pixel format to find/negotiate the resolution preset for
- * @type: M-5MOLS resolution type
- * @resolution:	M-5MOLS resolution preset register value
- *
- * Find nearest resolution matching resolution preset and adjust mf
- * to supported values.
- */
-static int __find_resolution(struct v4l2_subdev *sd,
-			     struct v4l2_mbus_framefmt *mf,
-			     enum m5mols_restype *type,
-			     u32 *resolution)
+static int m5mols_g_mbus_fmt(struct v4l2_subdev *sd,
+		struct v4l2_mbus_framefmt *ffmt)
 {
-	const struct m5mols_resolution *fsize = &m5mols_reg_res[0];
-	const struct m5mols_resolution *match = NULL;
-	enum m5mols_restype stype = __find_restype(mf->code);
-	int i = ARRAY_SIZE(m5mols_reg_res);
-	unsigned int min_err = ~0;
+	struct m5mols_info *info = to_m5mols(sd);
+	enum m5mols_res_type res_type;
 
-	while (i--) {
-		int err;
-		if (stype == fsize->type) {
-			err = abs(fsize->width - mf->width)
-				+ abs(fsize->height - mf->height);
+	res_type = to_res_type(sd, ffmt->code);
+	if (res_type == M5MOLS_RES_MAX)
+		return -EINVAL;
 
-			if (err < min_err) {
-				min_err = err;
-				match = fsize;
-			}
-		}
-		fsize++;
-	}
-	if (match) {
-		mf->width  = match->width;
-		mf->height = match->height;
-		*resolution = match->reg;
-		*type = stype;
-		return 0;
-	}
+	*ffmt = info->fmt[res_type];
+	info->code = ffmt->code;
 
-	return -EINVAL;
+	return 0;
 }
 
-static struct v4l2_mbus_framefmt *__find_format(struct m5mols_info *info,
-				struct v4l2_subdev_fh *fh,
-				enum v4l2_subdev_format_whence which,
-				enum m5mols_restype type)
+static int m5mols_into_monitor(struct v4l2_subdev *sd, int res_size)
 {
-	if (which == V4L2_SUBDEV_FORMAT_TRY)
-		return fh ? v4l2_subdev_get_try_format(fh, 0) : NULL;
+	int ret;
 
-	return &info->ffmt[type];
+	ret = m5mols_set_mode(sd, MODE_PARMSET);
+	if (!ret)
+		ret = i2c_w8_param(sd, CAT1_MONITOR_SIZE, (u8)res_size);
+	if (!ret)
+		ret = m5mols_set_mode(sd, MODE_PARMSET);
+
+	return ret;
 }
 
-static int m5mols_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
-			  struct v4l2_subdev_format *fmt)
+static int m5mols_into_capture(struct v4l2_subdev *sd, int res_size)
 {
 	struct m5mols_info *info = to_m5mols(sd);
-	struct v4l2_mbus_framefmt *format;
+	u32 reg;
+	int ret, timeout = 1;
+	u32 temp = 0;
 
-	if (fmt->pad != 0)
-		return -EINVAL;
+	info->captured = false;
 
-	format = __find_format(info, fh, fmt->which, info->res_type);
-	if (!format)
-		return -EINVAL;
+	/*
+	 * The sequence of preparing Capture mode.
+	 * 1. Clear Interrupt bit (for dummy)
+	 * 2. Enable Capture bit at Interrupt
+	 * 3. Lock AE/AWB
+	 * 4. Enter Still Capture mode
+	 */
 
-	fmt->format = *format;
-	return 0;
+	ret = m5mols_set_mode(sd, MODE_MONITOR);
+	if (!ret)
+		/* FIXME: setting capture exposure at the middle of a amount. */
+		ret = i2c_w16_ae(sd, CAT3_MANUAL_GAIN_CAP, 0x90);
+	if (!ret)
+		ret = m5mols_set_ae_lock(info, true);
+	if (!ret)
+		ret = m5mols_set_awb_lock(info, true);
+	if (!ret)
+		ret = i2c_r8_system(sd, CAT0_INT_FACTOR, &reg);
+	if (!ret)
+		ret = i2c_w8_system(sd, CAT0_INT_ENABLE, 1 << INT_BIT_CAPTURE);
+	if (!ret)
+		ret = m5mols_set_mode(sd, MODE_CAPTURE);
+	if (!ret)
+		timeout = wait_event_interruptible_timeout(info->cap_wait,
+				info->captured, msecs_to_jiffies(2000));
+
+	/* disable all interrupt & clear interrupt */
+	ret = i2c_w8_system(sd, CAT0_INT_ENABLE, 0x0);
+	if (!ret)
+		ret = i2c_r8_system(sd, CAT0_INT_FACTOR, &reg);
+	if (ret)
+		return -EPERM;
+
+	/* If all timeout exhausted, return error. */
+	if (!timeout)
+		return -ETIMEDOUT;
+
+	ret = i2c_r32_capt_ctrl(sd, CATC_CAP_IMAGE_SIZE, &temp);
+	info->captured = false;
+	return ret;
 }
 
-static int m5mols_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
-			  struct v4l2_subdev_format *fmt)
+static int m5mols_s_mbus_fmt(struct v4l2_subdev *sd,
+		struct v4l2_mbus_framefmt *ffmt)
 {
 	struct m5mols_info *info = to_m5mols(sd);
-	struct v4l2_mbus_framefmt *format = &fmt->format;
-	struct v4l2_mbus_framefmt *sfmt;
-	enum m5mols_restype type;
-	u32 resolution = 0;
-	int ret;
+	enum m5mols_res_type res_type;
+	int size;
+	int ret = -EINVAL;
 
-	if (fmt->pad != 0)
+	res_type = to_res_type(sd, ffmt->code);
+	if (res_type == M5MOLS_RES_MAX)
 		return -EINVAL;
 
-	ret = __find_resolution(sd, format, &type, &resolution);
-	if (ret < 0)
-		return ret;
+	/* If user set portrait for preview, it is substitued width width height
+	 * unless get_res_preset will fail that M5MOLS did not support
+	 * reverse WVGA */
+	if (ffmt->width < ffmt->height) {
+		int temp;
+		temp  = ffmt->width;
+		ffmt->width = ffmt->height;
+		ffmt->height = temp;
+	}
+	size = get_res_preset(sd, ffmt->width, ffmt->height, res_type);
+	if (size < 0)
+		return -EINVAL;
 
-	sfmt = __find_format(info, fh, fmt->which, type);
-	if (!sfmt)
-		return 0;
+	if (ffmt->code == m5mols_formats[M5MOLS_RES_MON].code)
+		ret = m5mols_into_monitor(sd, size);
+	else
+		ret = m5mols_into_capture(sd, 0);
 
-	*sfmt		= m5mols_default_ffmt[type];
-	sfmt->width	= format->width;
-	sfmt->height	= format->height;
+	info->fmt[res_type]		= default_fmt[res_type];
+	info->fmt[res_type].width	= ffmt->width;
+	info->fmt[res_type].height	= ffmt->height;
 
-	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
-		info->resolution = resolution;
-		info->code = format->code;
-		info->res_type = type;
-	}
+	*ffmt = info->fmt[res_type];
+	info->code = ffmt->code;
 
-	return 0;
+	return ret;
 }
 
-static int m5mols_enum_mbus_code(struct v4l2_subdev *sd,
-				 struct v4l2_subdev_fh *fh,
-				 struct v4l2_subdev_mbus_code_enum *code)
+static int m5mols_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index,
+			      enum v4l2_mbus_pixelcode *code)
 {
-	if (!code || code->index >= SIZE_DEFAULT_FFMT)
+	if (!code || index >= ARRAY_SIZE(m5mols_formats))
 		return -EINVAL;
 
-	code->code = m5mols_default_ffmt[code->index].code;
+	*code = m5mols_formats[index].code;
 
 	return 0;
 }
 
-static struct v4l2_subdev_pad_ops m5mols_pad_ops = {
-	.enum_mbus_code	= m5mols_enum_mbus_code,
-	.get_fmt	= m5mols_get_fmt,
-	.set_fmt	= m5mols_set_fmt,
-};
+static int m5mols_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	struct v4l2_captureparm *cp = &parms->parm.capture;
 
-/**
- * m5mols_sync_controls - Apply default scene mode and the current controls
- *
- * This is used only streaming for syncing between v4l2_ctrl framework and
- * m5mols's controls. First, do the scenemode to the sensor, then call
- * v4l2_ctrl_handler_setup. It can be same between some commands and
- * the scenemode's in the default v4l2_ctrls. But, such commands of control
- * should be prior to the scenemode's one.
- */
-int m5mols_sync_controls(struct m5mols_info *info)
+	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+			parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	cp->capability = V4L2_CAP_TIMEPERFRAME;
+	cp->timeperframe = info->tpf;
+
+	return 0;
+}
+
+static int m5mols_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
 {
+	struct m5mols_info *info = to_m5mols(sd);
+	struct v4l2_captureparm *cp = &parms->parm.capture;
 	int ret = -EINVAL;
 
-	if (!is_ctrl_synced(info)) {
-		ret = m5mols_do_scenemode(info, REG_SCENE_NORMAL);
-		if (ret)
-			return ret;
+	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+			parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
 
-		v4l2_ctrl_handler_setup(&info->handle);
-		info->ctrl_sync = true;
+	ret = m5mols_set_mode_backup(sd, MODE_PARMSET);
+	if (!ret)
+		ret = get_fps(sd, cp);	/* set right FPS to denominator. */
+	if (!ret)
+		ret = i2c_w8_param(sd, CAT1_MONITOR_FPS,
+				m5mols_reg_fps[cp->timeperframe.denominator]);
+	if (!ret)
+		ret = m5mols_set_mode_restore(sd);
+	if (!ret) {
+		cp->capability = V4L2_CAP_TIMEPERFRAME;
+		info->tpf = cp->timeperframe;
 	}
 
+	v4l2msg("denominator: %d / numerator: %d.\n",
+		cp->timeperframe.denominator, cp->timeperframe.numerator);
+
 	return ret;
 }
 
-/**
- * m5mols_start_monitor - Start the monitor mode
- *
- * Before applying the controls setup the resolution and frame rate
- * in PARAMETER mode, and then switch over to MONITOR mode.
- */
-static int m5mols_start_monitor(struct m5mols_info *info)
+static int m5mols_get_info_capture(struct v4l2_subdev *sd)
 {
-	struct v4l2_subdev *sd = &info->sd;
-	int ret;
+	struct m5mols_info *info = to_m5mols(sd);
+	struct m5mols_exif *exif = &info->cap.exif;
+	int denominator, numerator;
+	int ret = 0;
+
+	ret = i2c_r32_exif(sd, CAT7_INFO_EXPTIME_NU, &numerator);
+	if (!ret)
+		ret = i2c_r32_exif(sd, CAT7_INFO_EXPTIME_DE, &denominator);
+	if (!ret)
+		exif->exposure_time = (u32)(numerator / denominator);
+	if (ret)
+		return ret;
+
+	ret = i2c_r32_exif(sd, CAT7_INFO_TV_NU, &numerator);
+	if (!ret)
+		ret = i2c_r32_exif(sd, CAT7_INFO_TV_DE, &denominator);
+	if (!ret)
+		exif->shutter_speed = (u32)(numerator / denominator);
+	if (ret)
+		return ret;
 
-	ret = m5mols_mode(info, REG_PARAMETER);
+	ret = i2c_r32_exif(sd, CAT7_INFO_AV_NU, &numerator);
 	if (!ret)
-		ret = m5mols_write(sd, PARM_MON_SIZE, info->resolution);
+		ret = i2c_r32_exif(sd, CAT7_INFO_AV_DE, &denominator);
 	if (!ret)
-		ret = m5mols_write(sd, PARM_MON_FPS, REG_FPS_30);
+		exif->aperture = (u32)(numerator / denominator);
+	if (ret)
+		return ret;
+
+	ret = i2c_r32_exif(sd, CAT7_INFO_BV_NU, &numerator);
+	if (!ret)
+		ret = i2c_r32_exif(sd, CAT7_INFO_BV_DE, &denominator);
+	if (!ret)
+		exif->brightness = (u32)(numerator / denominator);
+	if (ret)
+		return ret;
+
+	ret = i2c_r32_exif(sd, CAT7_INFO_EBV_NU, &numerator);
+	if (!ret)
+		ret = i2c_r32_exif(sd, CAT7_INFO_EBV_DE, &denominator);
+	if (!ret)
+		exif->exposure_bias = (u32)(numerator / denominator);
+	if (ret)
+		return ret;
+
+	ret = i2c_r16_exif(sd, CAT7_INFO_ISO, (u32 *)&exif->iso_speed);
+	if (!ret)
+		ret = i2c_r16_exif(sd, CAT7_INFO_FLASH, (u32 *)&exif->flash);
+	if (!ret)
+		ret = i2c_r16_exif(sd, CAT7_INFO_SDR, (u32 *)&exif->sdr);
+	if (!ret)
+		ret = i2c_r16_exif(sd, CAT7_INFO_QVAL, (u32 *)&exif->qval);
+	if (ret)
+		return ret;
 	if (!ret)
-		ret = m5mols_mode(info, REG_MONITOR);
+		ret = i2c_r32_capt_ctrl(sd, CATC_CAP_IMAGE_SIZE,
+				&info->cap.main);
 	if (!ret)
-		ret = m5mols_sync_controls(info);
+		ret = i2c_r32_capt_ctrl(sd, CATC_CAP_THUMB_SIZE,
+				&info->cap.thumb);
+
+	info->cap.total = info->cap.main + info->cap.thumb;
+
+	v4l2_info(sd, "%s: capture total size %d\n", __func__, info->cap.total);
+	v4l2_info(sd, "%s: capture main size %d\n", __func__, info->cap.main);
+	v4l2_info(sd, "%s: capture thumb size %d\n", __func__, info->cap.thumb);
+	v4l2_info(sd, "%s: exposure_time %d\n", __func__, exif->exposure_time);
+	v4l2_info(sd, "%s: shutter_speed %d\n", __func__, exif->shutter_speed);
+	v4l2_info(sd, "%s: aperture %d\n", __func__, exif->aperture);
+	v4l2_info(sd, "%s: brightness %d\n", __func__, exif->brightness);
+	v4l2_info(sd, "%s: exposure_bias %d\n", __func__, exif->exposure_bias);
+	v4l2_info(sd, "%s: iso_speed %d\n", __func__, exif->iso_speed);
+	v4l2_info(sd, "%s: flash %d\n", __func__, exif->flash);
+	v4l2_info(sd, "%s: sdr %d\n", __func__, exif->sdr);
+	v4l2_info(sd, "%s: qval %d\n", __func__, exif->qval);
 
 	return ret;
 }
 
-static int m5mols_s_stream(struct v4l2_subdev *sd, int enable)
+/* TODO: not verified. */
+static int m5mols_start_capture(struct v4l2_subdev *sd)
 {
 	struct m5mols_info *info = to_m5mols(sd);
+	u32 reg, size;
+	int ret, timeout;
+
+	u8 reg_capt_fmt[] = {
+		0x10,	/* JPEG with header + Thumbnail JPEG(YUV422@QVGA) */
+	}; /* YUV422, JPEG(422), JPEG(420) */
+	info->captured = false;
+
+	size = get_res_preset(sd,
+			info->fmt[M5MOLS_RES_CAPTURE].width,
+			info->fmt[M5MOLS_RES_CAPTURE].height,
+			M5MOLS_RES_CAPTURE);
+	if (size < 0)
+		return -EINVAL;
+	ret = 0;
+	/*
+	 * The sequence of Starting Capture mode.
+	 * 1. Select capture Single or Multi
+	 * 2. Select format (YUV422, JPEG(YUV420, YUV422))
+	 * 3. Set image size preset of Capture
+	 * 4. Read Interrupt bit (for dummy)
+	 * 5. Enable Capture bit at Interrupt
+	 * 6. Start Capture
+	 * 7. Check interrupt and register value
+	 * 8. Get Image & Thumb size
+	 */
+	ret = i2c_w8_capt_ctrl(sd, CATC_CAP_SEL_FRAME, true);	/* single capture */
+	if (!ret)
+		ret = i2c_w8_capt_parm(sd, CATB_YUVOUT_MAIN, reg_capt_fmt[0]);
+	if (!ret)
+		ret = i2c_w8_capt_parm(sd, CATB_MAIN_IMAGE_SIZE, size);
+	if (!ret)
+		ret = i2c_r8_system(sd, CAT0_INT_FACTOR, &reg);
+	if (!ret)
+		ret = i2c_w8_system(sd, CAT0_INT_ENABLE, 1 << INT_BIT_CAPTURE);
+	if (!ret)
+		ret = i2c_w8_capt_ctrl(sd, CATC_CAP_START, true);
+	if (!ret) {
+		timeout = wait_event_interruptible_timeout(info->cap_wait,
+				info->captured, msecs_to_jiffies(2000));
 
-	if (enable) {
-		int ret = -EINVAL;
+		if (info->captured) {
+			ret = m5mols_get_info_capture(sd);
+
+			if (!ret)
+				v4l2_subdev_notify(sd, info->cap.total, NULL);
+			else
+				return ret;
+		}
+
+		/* disable all interrupt & clear interrupt */
+		ret = i2c_w8_system(sd, CAT0_INT_ENABLE, 0x0);
+		if (!ret)
+			ret = i2c_r8_system(sd, CAT0_INT_FACTOR, &reg);
+		if (ret)
+			return -EPERM;
 
-		if (is_code(info->code, M5MOLS_RESTYPE_MONITOR))
-			ret = m5mols_start_monitor(info);
-		if (is_code(info->code, M5MOLS_RESTYPE_CAPTURE))
-			ret = m5mols_start_capture(info);
+		/* If all timeout exhausted, return error. */
+		if (!timeout)
+			return -ETIMEDOUT;
 
-		return ret;
+		info->captured = false;
+
+		ret = 0;
 	}
 
-	return m5mols_mode(info, REG_PARAMETER);
+	/* TODO: complete capture. */
+
+	return ret;
+}
+
+static int m5mols_start_monitor(struct v4l2_subdev *sd)
+{
+	return m5mols_set_mode(sd, MODE_MONITOR);
+}
+
+static int m5mols_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+
+	if (enable) {
+		if (info->code == to_code(M5MOLS_RES_MON)) {
+			v4l2_info(sd, "%s : monitor mode\n", __func__);
+			return m5mols_start_monitor(sd);
+		}
+		if (info->code == to_code(M5MOLS_RES_CAPTURE)) {
+			v4l2_info(sd, "%s : capture mode\n", __func__);
+			return  m5mols_start_capture(sd);
+		}
+		return -EINVAL;
+	} else {
+		if (is_streaming(sd))
+			return m5mols_set_mode(sd, MODE_PARMSET);
+		return -EINVAL;
+	}
 }
 
 static const struct v4l2_subdev_video_ops m5mols_video_ops = {
-	.s_stream	= m5mols_s_stream,
+	.g_mbus_fmt		= m5mols_g_mbus_fmt,
+	.s_mbus_fmt		= m5mols_s_mbus_fmt,
+	.enum_mbus_fmt		= m5mols_enum_mbus_fmt,
+	.g_parm			= m5mols_g_parm,
+	.s_parm			= m5mols_s_parm,
+	.s_stream		= m5mols_s_stream,
 };
 
 static int m5mols_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct v4l2_subdev *sd = to_sd(ctrl);
-	struct m5mols_info *info = to_m5mols(sd);
 	int ret;
 
-	info->mode_save = info->mode;
-
-	ret = m5mols_mode(info, REG_PARAMETER);
+	ret = m5mols_set_mode_backup(sd, MODE_PARMSET);
 	if (!ret)
 		ret = m5mols_set_ctrl(ctrl);
 	if (!ret)
-		ret = m5mols_mode(info, info->mode_save);
+		ret = m5mols_set_mode_restore(sd);
 
 	return ret;
 }
 
+static int m5mols_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+	struct m5mols_info *info = to_m5mols(sd);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_CAM_JPEG_ENCODEDSIZE:
+		ctrl->cur.val = info->cap.total;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
 static const struct v4l2_ctrl_ops m5mols_ctrl_ops = {
-	.s_ctrl	= m5mols_s_ctrl,
+	.s_ctrl = m5mols_s_ctrl,
+	.g_volatile_ctrl = m5mols_g_volatile_ctrl,
 };
 
+static const struct v4l2_ctrl_config ctrl_private[] = {
+	{
+		.ops = &m5mols_ctrl_ops,
+		.id = V4L2_CID_CAM_JPEG_MEMSIZE,
+		.name = "Jpeg memory size",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.flags = V4L2_CTRL_FLAG_SLIDER,
+		.max = M5MO_JPEG_MEMSIZE,
+		.step = 1,
+		.min = 0,
+		.def = M5MO_JPEG_MEMSIZE,
+		.is_private = 1,
+	}, {
+		.ops = &m5mols_ctrl_ops,
+		.id = V4L2_CID_CAM_JPEG_ENCODEDSIZE,
+		.name = "Jpeg encoded size",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.flags = V4L2_CTRL_FLAG_SLIDER,
+		.max = M5MO_JPEG_MEMSIZE,
+		.step = 1,
+		.min = 0,
+		.def = 0,
+		.is_private = 1,
+		.is_volatile = 1,
+	},
+};
+/*
+ * m5mols_sensor_power - handle sensor power up/down.
+ *
+ * @enable: If it is true, power up. If is not, power down.
+ */
 static int m5mols_sensor_power(struct m5mols_info *info, bool enable)
 {
 	struct v4l2_subdev *sd = &info->sd;
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	const struct m5mols_platform_data *pdata = info->pdata;
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
 	int ret;
 
 	if (enable) {
-		if (is_powered(info))
+		if (is_powerup(sd))
 			return 0;
 
+		/* power-on additional power */
 		if (info->set_power) {
-			ret = info->set_power(&client->dev, 1);
+			ret = info->set_power(&c->dev, 1);
 			if (ret)
 				return ret;
 		}
 
 		ret = regulator_bulk_enable(ARRAY_SIZE(supplies), supplies);
-		if (ret) {
-			info->set_power(&client->dev, 0);
+		if (ret)
 			return ret;
-		}
 
-		gpio_set_value(pdata->gpio_reset, !pdata->reset_polarity);
+		gpio_set_value(info->pdata->gpio_rst, info->pdata->enable_rst);
 		usleep_range(1000, 1000);
-		info->power = true;
 
-		return ret;
-	}
+		info->power = true;
+	} else {
+		if (!is_powerup(sd))
+			return 0;
 
-	if (!is_powered(info))
-		return 0;
+		ret = regulator_bulk_disable(ARRAY_SIZE(supplies), supplies);
+		if (ret)
+			return ret;
 
-	ret = regulator_bulk_disable(ARRAY_SIZE(supplies), supplies);
-	if (ret)
-		return ret;
+		/* power-off additional power */
+		if (info->set_power) {
+			ret = info->set_power(&c->dev, 0);
+			if (ret)
+				return ret;
+		}
 
-	if (info->set_power)
-		info->set_power(&client->dev, 0);
+		info->power = false;
 
-	gpio_set_value(pdata->gpio_reset, pdata->reset_polarity);
-	usleep_range(1000, 1000);
-	info->power = false;
+		gpio_set_value(info->pdata->gpio_rst, !info->pdata->enable_rst);
+		usleep_range(1000, 1000);
+	}
 
 	return ret;
 }
 
-/* m5mols_update_fw - optional firmware update routine */
-int __attribute__ ((weak)) m5mols_update_fw(struct v4l2_subdev *sd,
-		int (*set_power)(struct m5mols_info *, bool))
+static void m5mols_irq_work(struct work_struct *work)
 {
-	return 0;
+	struct m5mols_info *info = container_of(work, struct m5mols_info, work);
+	struct v4l2_subdev *sd = &info->sd;
+	u32 reg;
+	int ret;
+	if (is_powerup(sd)) {
+		ret = i2c_r8_system(sd, CAT0_INT_FACTOR, &reg);
+		if (!ret) {
+			switch (reg & 0x0f) {
+			case (1 << INT_BIT_AF):
+				/* Except returning zero at just that upper
+				 * statments, not entering in this parenthesis.
+				 * The return value is below:
+				 * 0x0 : AF Fail
+				 * 0x2 : AF Success
+				 * 0x4 : Idle Status
+				 * 0x5 : Busy Status */
+				ret = i2c_r8_lens(sd, CATA_AF_STATUS, &reg);
+				if (!ret && (reg == 0x02))
+					info->is_focus = true;
+				else
+					info->is_focus = false;
+				printk("%s = AF %02x, focus %d\n",
+						__func__, reg, info->is_focus);
+				break;
+			case (1 << INT_BIT_CAPTURE):
+				printk("%s = CAPTURE\n", __func__);
+				if (!info->captured) {
+					wake_up_interruptible(&info->cap_wait);
+					info->captured = true;
+				}
+				break;
+			case (1 << INT_BIT_ZOOM):
+			case (1 << INT_BIT_FRAME_SYNC):
+			case (1 << INT_BIT_FD):
+			case (1 << INT_BIT_LENS_INIT):
+			case (1 << INT_BIT_SOUND):
+				printk("%s = Nothing : 0x%08x\n", __func__, reg);
+				break;
+			case (1 << INT_BIT_MODE):
+			default:
+				break;
+			}
+		}
+	}
 }
 
-/**
- * m5mols_sensor_armboot - Booting M-5MOLS internal ARM core.
+static irqreturn_t m5mols_irq_handler(int irq, void *data)
+{
+	struct v4l2_subdev *sd = data;
+	struct m5mols_info *info = to_m5mols(sd);
+
+	v4l2_info(sd, "%s\n", __func__);
+
+	schedule_work(&info->work);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * m5mols_sensor_armboot - booting M5MOLS internal ARM core-controller.
  *
- * Booting internal ARM core makes the M-5MOLS is ready for getting commands
- * with I2C. It's the first thing to be done after it powered up. It must wait
- * at least 520ms recommended by M-5MOLS datasheet, after executing arm booting.
+ * It makes to ready M5MOLS for I2C & MIPI interface. After it's powered up,
+ * it activates if it gets armboot command for I2C interface. After getting
+ * cmd, it must wait about least 500ms referenced by M5MOLS datasheet.
  */
 static int m5mols_sensor_armboot(struct v4l2_subdev *sd)
 {
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct m5mols_info *info = to_m5mols(sd);
+	static u8 m5mols_mipi_value = 0x02;
+	u32 reg;
 	int ret;
 
-	ret = m5mols_write(sd, FLASH_CAM_START, REG_START_ARM_BOOT);
+	/* 1. ARM booting */
+	ret = i2c_w8_flash(sd, CATC_CAM_START, true);
 	if (ret < 0)
 		return ret;
 
-	msleep(520);
-
-	ret = m5mols_get_version(sd);
-	if (!ret)
-		ret = m5mols_update_fw(sd, m5mols_sensor_power);
-	if (ret)
-		return ret;
+	msleep(500);
+	dev_dbg(&client->dev, "Success ARM Booting\n");
 
-	v4l2_dbg(1, m5mols_debug, sd, "Success ARM Booting\n");
+	/* after ARM booting, the M5MOLS state changed Parameter mode. */
+	info->mode = MODE_PARMSET;
 
-	ret = m5mols_write(sd, PARM_INTERFACE, REG_INTERFACE_MIPI);
+	ret = i2c_r8_system(sd, CAT0_INT_FACTOR, &reg);		/* clear intterupt */
 	if (!ret)
-		ret = m5mols_enable_interrupt(sd, REG_INT_AF);
+		ret = i2c_w8_system(sd, CAT0_INT_ENABLE, 0x0);	/* all disable */
+	if (!ret)
+		ret = get_version(sd);
+	if (!ret)
+		ret = i2c_w8_param(sd, CAT1_DATA_INTERFACE, m5mols_mipi_value);
+
+	m5mols_show_version(sd);
 
 	return ret;
 }
 
+/*
+ * m5mols_init_controls - initialization using v4l2_ctrl.
+ */
 static int m5mols_init_controls(struct m5mols_info *info)
 {
 	struct v4l2_subdev *sd = &info->sd;
-	u16 max_exposure;
-	u16 step_zoom;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 max_ex_mon;
 	int ret;
 
-	/* Determine value's range & step of controls for various FW version */
-	ret = m5mols_read_u16(sd, AE_MAX_GAIN_MON, &max_exposure);
-	if (!ret)
-		step_zoom = is_manufacturer(info, REG_SAMSUNG_OPTICS) ? 31 : 1;
+	/* check minimum & maximum of M5MOLS controls */
+	ret = i2c_r16_ae(sd, CAT3_MAX_GAIN_MON, (u32 *)&max_ex_mon);
 	if (ret)
 		return ret;
 
-	v4l2_ctrl_handler_init(&info->handle, 6);
+	/* set the controls using v4l2 control frameworks */
+	v4l2_ctrl_handler_init(&info->handle, 9);
+
+	info->colorfx = v4l2_ctrl_new_std_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_COLORFX,
+			9, 1, V4L2_COLORFX_NONE);
+	info->autoexposure = v4l2_ctrl_new_std_menu(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE_AUTO,
+			1, 0, V4L2_EXPOSURE_AUTO);
+	info->exposure = v4l2_ctrl_new_std(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE,
+			0, max_ex_mon, 1, (int)max_ex_mon/2);
+	info->autofocus = v4l2_ctrl_new_std(&info->handle,
+			&m5mols_ctrl_ops, V4L2_CID_FOCUS_AUTO,
+			0, 1, 1, 0);
 	info->autowb = v4l2_ctrl_new_std(&info->handle,
 			&m5mols_ctrl_ops, V4L2_CID_AUTO_WHITE_BALANCE,
-			0, 1, 1, 0);
+			0, 1, 1, 1);
 	info->saturation = v4l2_ctrl_new_std(&info->handle,
 			&m5mols_ctrl_ops, V4L2_CID_SATURATION,
-			1, 5, 1, 3);
+			0, 6, 1, 3);
 	info->zoom = v4l2_ctrl_new_std(&info->handle,
 			&m5mols_ctrl_ops, V4L2_CID_ZOOM_ABSOLUTE,
-			1, 70, step_zoom, 1);
-	info->exposure = v4l2_ctrl_new_std(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE,
-			0, max_exposure, 1, (int)max_exposure/2);
-	info->colorfx = v4l2_ctrl_new_std_menu(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_COLORFX,
-			4, (1 << V4L2_COLORFX_BW), V4L2_COLORFX_NONE);
-	info->autoexposure = v4l2_ctrl_new_std_menu(&info->handle,
-			&m5mols_ctrl_ops, V4L2_CID_EXPOSURE_AUTO,
-			1, 0, V4L2_EXPOSURE_MANUAL);
+			0, 70, 1, 0);
+	info->jpeg_size = v4l2_ctrl_new_custom(&info->handle,
+			&ctrl_private[0],
+			NULL);
+	info->encoded_size = v4l2_ctrl_new_custom(&info->handle,
+			&ctrl_private[1],
+			NULL);
 
 	sd->ctrl_handler = &info->handle;
+
 	if (info->handle.error) {
-		v4l2_err(sd, "Failed to initialize controls: %d\n", ret);
+		dev_err(&client->dev, "Failed to init controls, %d\n", ret);
 		v4l2_ctrl_handler_free(&info->handle);
 		return info->handle.error;
 	}
 
 	v4l2_ctrl_cluster(2, &info->autoexposure);
+	/* If above ctrl value is not good image, so it is better that not set */
+	v4l2_ctrl_handler_setup(&info->handle);
 
 	return 0;
 }
 
-/**
- * m5mols_s_power - Main sensor power control function
- *
- * To prevent breaking the lens when the sensor is powered off the Soft-Landing
- * algorithm is called where available. The Soft-Landing algorithm availability
- * dependends on the firmware provider.
+/*
+ * m5mols_setup_default - set default size & fps in the monitor mode.
  */
+static int m5mols_setup_default(struct v4l2_subdev *sd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	int value;
+	int ret = -EINVAL;
+
+	value = get_res_preset(sd,
+			default_fmt[M5MOLS_RES_MON].width,
+			default_fmt[M5MOLS_RES_MON].height,
+			M5MOLS_RES_MON);
+	if (value >= 0)
+		ret = i2c_w8_param(sd, CAT1_MONITOR_SIZE, (u8)value);
+	if (!ret)
+		ret = i2c_w8_param(sd, CAT1_MONITOR_FPS,
+			m5mols_reg_fps[default_fps.denominator]);
+	if (!ret)
+		ret = m5mols_init_controls(info);
+	if (!ret)
+		ret = m5mols_set_ae_lock(info, false);
+	if (!ret)
+		ret = m5mols_set_awb_lock(info, false);
+	if (!ret) {
+		info->fmt[M5MOLS_RES_MON] = default_fmt[M5MOLS_RES_MON];
+		info->tpf = default_fps;
+
+		ret = 0;
+	}
+
+	return ret;
+}
+
 static int m5mols_s_power(struct v4l2_subdev *sd, int on)
 {
 	struct m5mols_info *info = to_m5mols(sd);
@@ -815,34 +1233,9 @@ static int m5mols_s_power(struct v4l2_subdev *sd, int on)
 		if (!ret)
 			ret = m5mols_sensor_armboot(sd);
 		if (!ret)
-			ret = m5mols_init_controls(info);
-		if (ret)
-			return ret;
-
-		info->ffmt[M5MOLS_RESTYPE_MONITOR] =
-			m5mols_default_ffmt[M5MOLS_RESTYPE_MONITOR];
-		info->ffmt[M5MOLS_RESTYPE_CAPTURE] =
-			m5mols_default_ffmt[M5MOLS_RESTYPE_CAPTURE];
-		return ret;
-	}
-
-	if (is_manufacturer(info, REG_SAMSUNG_TECHWIN)) {
-		ret = m5mols_mode(info, REG_MONITOR);
-		if (!ret)
-			ret = m5mols_write(sd, AF_EXECUTE, REG_AF_STOP);
-		if (!ret)
-			ret = m5mols_write(sd, AF_MODE, REG_AF_POWEROFF);
-		if (!ret)
-			ret = m5mols_busy(sd, CAT_SYSTEM, CAT0_STATUS,
-					REG_AF_IDLE);
-		if (!ret)
-			v4l2_info(sd, "Success soft-landing lens\n");
-	}
-
-	ret = m5mols_sensor_power(info, false);
-	if (!ret) {
-		v4l2_ctrl_handler_free(&info->handle);
-		info->ctrl_sync = false;
+			ret = m5mols_setup_default(sd);
+	} else {
+		ret = m5mols_sensor_power(info, false);
 	}
 
 	return ret;
@@ -858,168 +1251,334 @@ static int m5mols_log_status(struct v4l2_subdev *sd)
 }
 
 static const struct v4l2_subdev_core_ops m5mols_core_ops = {
-	.s_power	= m5mols_s_power,
-	.g_ctrl		= v4l2_subdev_g_ctrl,
-	.s_ctrl		= v4l2_subdev_s_ctrl,
-	.queryctrl	= v4l2_subdev_queryctrl,
-	.querymenu	= v4l2_subdev_querymenu,
-	.g_ext_ctrls	= v4l2_subdev_g_ext_ctrls,
-	.try_ext_ctrls	= v4l2_subdev_try_ext_ctrls,
-	.s_ext_ctrls	= v4l2_subdev_s_ext_ctrls,
-	.log_status	= m5mols_log_status,
+	.s_power		= m5mols_s_power,
+	.g_ctrl			= v4l2_subdev_g_ctrl,
+	.s_ctrl			= v4l2_subdev_s_ctrl,
+	.queryctrl		= v4l2_subdev_queryctrl,
+	.querymenu		= v4l2_subdev_querymenu,
+	.g_ext_ctrls		= v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls		= v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls		= v4l2_subdev_s_ext_ctrls,
+	.log_status		= m5mols_log_status,
+};
+
+/**
+ * __find_restype - Lookup M-5MOLS resolution type according to pixel code
+ * @code: pixel code
+ */
+static enum m5mols_restype __find_restype(enum v4l2_mbus_pixelcode code)
+{
+	enum m5mols_restype type = M5MOLS_RESTYPE_MONITOR;
+
+	do {
+		if (code == default_fmt[type].code)
+			return type;
+	} while (type++ != SIZE_DEFAULT_FFMT);
+
+	return 0;
+}
+
+/**
+ * __find_resolution - Lookup preset and type of M-5MOLS's resolution
+ * @mf: pixel format to find/negotiate the resolution preset for
+ * @type: M-5MOLS resolution type
+ * @resolution:	M-5MOLS resolution preset register value
+ *
+ * Find nearest resolution matching resolution preset and adjust mf
+ * to supported values.
+ */
+static int __find_resolution(struct v4l2_subdev *sd,
+			     struct v4l2_mbus_framefmt *mf,
+			     enum m5mols_restype *type,
+			     u32 *resolution)
+{
+	const struct m5mols_resolution *fsize = &m5mols_resolutions[0];
+	const struct m5mols_resolution *match = NULL;
+	enum m5mols_restype stype = __find_restype(mf->code);
+	int i = ARRAY_SIZE(m5mols_resolutions);
+	unsigned int min_err = ~0;
+
+	while (i--) {
+		int err;
+		if (stype == fsize->type) {
+			err = abs(fsize->width - mf->width)
+				+ abs(fsize->height - mf->height);
+
+			if (err < min_err) {
+				min_err = err;
+				match = fsize;
+			}
+		}
+		fsize++;
+	}
+	if (match) {
+		mf->width  = match->width;
+		mf->height = match->height;
+		*resolution = match->value;
+		*type = stype;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static struct v4l2_mbus_framefmt *__find_format(struct m5mols_info *info,
+				struct v4l2_subdev_fh *fh,
+				enum v4l2_subdev_format_whence which,
+				enum m5mols_restype type)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return fh ? v4l2_subdev_get_try_format(fh, 0) : NULL;
+
+	return &info->fmt[type];
+}
+
+static int m5mols_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (fmt->pad != 0)
+		return -EINVAL;
+
+	format = __find_format(info, fh, fmt->which, info->res_type);
+	if (!format)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+static int m5mols_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	struct v4l2_mbus_framefmt *format = &fmt->format;
+	struct v4l2_mbus_framefmt *sfmt;
+	enum m5mols_restype type;
+	u32 resolution = 0;
+	int ret;
+
+	if (fmt->pad != 0)
+		return -EINVAL;
+
+	ret = __find_resolution(sd, format, &type, &resolution);
+	if (ret < 0)
+		return ret;
+
+	sfmt = __find_format(info, fh, fmt->which, type);
+	if (!sfmt)
+		return 0;
+
+	sfmt		= &default_fmt[type];
+	sfmt->width	= format->width;
+	sfmt->height	= format->height;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		info->resolution = resolution;
+		info->code = format->code;
+		info->res_type = type;
+	}
+
+	return 0;
+}
+
+static int m5mols_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (!code || code->index >= SIZE_DEFAULT_FFMT)
+		return -EINVAL;
+
+	code->code = default_fmt[code->index].code;
+
+	return 0;
+}
+
+static struct v4l2_subdev_pad_ops m5mols_pad_ops = {
+	.enum_mbus_code	= m5mols_enum_mbus_code,
+	.get_fmt	= m5mols_get_fmt,
+	.set_fmt	= m5mols_set_fmt,
 };
 
 static const struct v4l2_subdev_ops m5mols_ops = {
-	.core		= &m5mols_core_ops,
-	.pad		= &m5mols_pad_ops,
-	.video		= &m5mols_video_ops,
+	.core	= &m5mols_core_ops,
+	.pad	= &m5mols_pad_ops,
+	.video	= &m5mols_video_ops,
 };
 
-static void m5mols_irq_work(struct work_struct *work)
+static int m5mols_link_setup(struct media_entity *entity,
+			    const struct media_pad *local,
+			    const struct media_pad *remote, u32 flags)
 {
-	struct m5mols_info *info =
-		container_of(work, struct m5mols_info, work_irq);
-	struct v4l2_subdev *sd = &info->sd;
-	u8 reg;
-	int ret;
+	printk("%s\n", __func__);
+	return 0;
+}
+static const struct media_entity_operations m5mols_media_ops = {
+	.link_setup = m5mols_link_setup,
+};
 
-	if (!is_powered(info) ||
-			m5mols_read_u8(sd, SYSTEM_INT_FACTOR, &info->interrupt))
-		return;
+static int m5mols_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
 
-	switch (info->interrupt & REG_INT_MASK) {
-	case REG_INT_AF:
-		if (!is_available_af(info))
-			break;
-		ret = m5mols_read_u8(sd, AF_STATUS, &reg);
-		v4l2_dbg(2, m5mols_debug, sd, "AF %s\n",
-			 reg == REG_AF_FAIL ? "Failed" :
-			 reg == REG_AF_SUCCESS ? "Success" :
-			 reg == REG_AF_IDLE ? "Idle" : "Busy");
-		break;
-	case REG_INT_CAPTURE:
-		if (!test_and_set_bit(ST_CAPT_IRQ, &info->flags))
-			wake_up_interruptible(&info->irq_waitq);
+	memset(&format, 0, sizeof(format));
+	format.pad = 0;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = m5mols_formats[M5MOLS_RES_MON].code;
+	format.format.width = DEFAULT_SENSOR_WIDTH;
+	format.format.height = DEFAULT_SENSOR_HEIGHT;
 
-		v4l2_dbg(2, m5mols_debug, sd, "CAPTURE\n");
-		break;
-	default:
-		v4l2_dbg(2, m5mols_debug, sd, "Undefined: %02x\n", reg);
-		break;
-	};
+	m5mols_set_fmt(sd, fh, &format);
+
+	return 0;
 }
 
-static irqreturn_t m5mols_irq_handler(int irq, void *data)
+static int m5mols_subdev_close(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_fh *fh)
 {
-	struct v4l2_subdev *sd = data;
-	struct m5mols_info *info = to_m5mols(sd);
+	v4l2_dbg(1, m5mols_debug, sd, "%s", __func__);
+	return 0;
+}
 
-	schedule_work(&info->work_irq);
+static int m5mols_subdev_registered(struct v4l2_subdev *sd)
+{
+	v4l2_dbg(1, m5mols_debug, sd, "%s", __func__);
+	return 0;
+}
 
-	return IRQ_HANDLED;
+static void m5mols_subdev_unregistered(struct v4l2_subdev *sd)
+{
+	v4l2_dbg(1, m5mols_debug, sd, "%s", __func__);
 }
 
-static int __devinit m5mols_probe(struct i2c_client *client,
-				  const struct i2c_device_id *id)
+static const struct v4l2_subdev_internal_ops m5mols_v4l2_internal_ops = {
+	.open = m5mols_init_formats,
+	.close = m5mols_subdev_close,
+	.registered = m5mols_subdev_registered,
+	.unregistered = m5mols_subdev_unregistered,
+};
+
+static int m5mols_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
 {
-	const struct m5mols_platform_data *pdata = client->dev.platform_data;
+	const struct m5mols_platform_data *pdata =
+		client->dev.platform_data;
 	struct m5mols_info *info;
 	struct v4l2_subdev *sd;
-	int ret;
+	int ret = 0;
 
 	if (pdata == NULL) {
 		dev_err(&client->dev, "No platform data\n");
 		return -EINVAL;
 	}
 
-	if (!gpio_is_valid(pdata->gpio_reset)) {
-		dev_err(&client->dev, "No valid RESET GPIO specified\n");
+	if (!gpio_is_valid(pdata->gpio_rst)) {
+		dev_err(&client->dev, "No valid nRST gpio pin.\n");
 		return -EINVAL;
 	}
 
 	if (!pdata->irq) {
-		dev_err(&client->dev, "Interrupt not assigned\n");
+		dev_err(&client->dev, "Interrupt not assigned.\n");
 		return -EINVAL;
 	}
 
 	info = kzalloc(sizeof(struct m5mols_info), GFP_KERNEL);
-	if (!info)
+	if (info == NULL) {
+		dev_err(&client->dev, "Failed to allocate info\n");
 		return -ENOMEM;
+	}
 
-	info->pdata = pdata;
-	info->set_power	= pdata->set_power;
+	info->pdata	= pdata;
+	if (info->pdata->set_power)	/* for additional power if needed. */
+		info->set_power = pdata->set_power;
+
+	if (info->pdata->irq) {
+		INIT_WORK(&info->work, m5mols_irq_work);
+		ret = request_irq(info->pdata->irq, m5mols_irq_handler,
+				  IRQF_TRIGGER_RISING, MOD_NAME, &info->sd);
+		if (ret) {
+			dev_err(&client->dev, "Failed to request irq: %d\n", ret);
+			return ret;
+		}
+	}
 
-	ret = gpio_request(pdata->gpio_reset, "M5MOLS_NRST");
+	ret = gpio_request(info->pdata->gpio_rst, "M5MOLS nRST");
 	if (ret) {
-		dev_err(&client->dev, "Failed to request gpio: %d\n", ret);
-		goto out_free;
+		dev_err(&client->dev, "Failed to set gpio, %d\n", ret);
+		goto out_gpio;
 	}
-	gpio_direction_output(pdata->gpio_reset, pdata->reset_polarity);
+
+	gpio_direction_output(info->pdata->gpio_rst, !info->pdata->enable_rst);
 
 	ret = regulator_bulk_get(&client->dev, ARRAY_SIZE(supplies), supplies);
 	if (ret) {
-		dev_err(&client->dev, "Failed to get regulators: %d\n", ret);
-		goto out_gpio;
+		dev_err(&client->dev, "Failed to get regulators, %d\n", ret);
+		goto out_reg;
 	}
 
 	sd = &info->sd;
-	strlcpy(sd->name, MODULE_NAME, sizeof(sd->name));
-	v4l2_i2c_subdev_init(sd, client, &m5mols_ops);
+	strlcpy(sd->name, MOD_NAME, sizeof(sd->name));
+
+	init_waitqueue_head(&info->cap_wait);
 
+	v4l2_i2c_subdev_init(sd, client, &m5mols_ops);
 	info->pad.flags = MEDIA_PAD_FL_SOURCE;
 	ret = media_entity_init(&sd->entity, 1, &info->pad, 0);
 	if (ret < 0)
 		goto out_reg;
+
+	m5mols_init_formats(sd, NULL);
+
 	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sd->internal_ops = &m5mols_v4l2_internal_ops;
+	sd->entity.ops = &m5mols_media_ops;
 
-	init_waitqueue_head(&info->irq_waitq);
-	INIT_WORK(&info->work_irq, m5mols_irq_work);
-	ret = request_irq(pdata->irq, m5mols_irq_handler,
-			  IRQF_TRIGGER_RISING, MODULE_NAME, sd);
-	if (ret) {
-		dev_err(&client->dev, "Interrupt request failed: %d\n", ret);
-		goto out_me;
-	}
 	info->res_type = M5MOLS_RESTYPE_MONITOR;
+
+	v4l2_info(sd, "%s : m5mols driver probed success\n", __func__);
+
 	return 0;
-out_me:
-	media_entity_cleanup(&sd->entity);
+
 out_reg:
 	regulator_bulk_free(ARRAY_SIZE(supplies), supplies);
 out_gpio:
-	gpio_free(pdata->gpio_reset);
-out_free:
+	gpio_free(info->pdata->gpio_rst);
 	kfree(info);
+
 	return ret;
 }
 
-static int __devexit m5mols_remove(struct i2c_client *client)
+static int m5mols_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	struct m5mols_info *info = to_m5mols(sd);
 
 	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&info->handle);
 	free_irq(info->pdata->irq, sd);
-
 	regulator_bulk_free(ARRAY_SIZE(supplies), supplies);
-	gpio_free(info->pdata->gpio_reset);
+	gpio_free(info->pdata->gpio_rst);
 	media_entity_cleanup(&sd->entity);
 	kfree(info);
+
 	return 0;
 }
 
 static const struct i2c_device_id m5mols_id[] = {
-	{ MODULE_NAME, 0 },
+	{ MOD_NAME, 0 },
 	{ },
 };
 MODULE_DEVICE_TABLE(i2c, m5mols_id);
 
 static struct i2c_driver m5mols_i2c_driver = {
 	.driver = {
-		.name	= MODULE_NAME,
+		.name	= MOD_NAME,
 	},
 	.probe		= m5mols_probe,
-	.remove		= __devexit_p(m5mols_remove),
+	.remove		= m5mols_remove,
 	.id_table	= m5mols_id,
 };
 
@@ -1038,5 +1597,5 @@ module_exit(m5mols_mod_exit);
 
 MODULE_AUTHOR("HeungJun Kim <riverful.kim@samsung.com>");
 MODULE_AUTHOR("Dongsoo Kim <dongsoo45.kim@samsung.com>");
-MODULE_DESCRIPTION("Fujitsu M-5MOLS 8M Pixel camera driver");
+MODULE_DESCRIPTION("Fujitsu M5MOLS 8M Pixel camera sensor with ISP driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/m5mols/m5mols_reg.h b/drivers/media/video/m5mols/m5mols_reg.h
index c755bd6..da9cab5 100644
--- a/drivers/media/video/m5mols/m5mols_reg.h
+++ b/drivers/media/video/m5mols/m5mols_reg.h
@@ -1,5 +1,5 @@
 /*
- * Register map for M-5MOLS 8M Pixel camera sensor with ISP
+ * Register map for M5MOLS 8M Pixel camera sensor with ISP
  *
  * Copyright (C) 2011 Samsung Electronics Co., Ltd.
  * Author: HeungJun Kim <riverful.kim@samsung.com>
@@ -16,258 +16,104 @@
 #ifndef M5MOLS_REG_H
 #define M5MOLS_REG_H
 
-#define M5MOLS_I2C_MAX_SIZE	4
-#define M5MOLS_BYTE_READ	0x01
-#define M5MOLS_BYTE_WRITE	0x02
-
-#define I2C_CATEGORY(__cat)		((__cat >> 16) & 0xff)
-#define I2C_COMMAND(__comm)		((__comm >> 8) & 0xff)
-#define I2C_SIZE(__reg_s)		((__reg_s) & 0xff)
-#define I2C_REG(__cat, __cmd, __reg_s)	((__cat << 16) | (__cmd << 8) | __reg_s)
-
 /*
  * Category section register
  *
- * The category means set including relevant command of M-5MOLS.
+ * The category means a kind of command set. Including category section,
+ * all defined categories in this version supports only, as you see below:
  */
 #define CAT_SYSTEM		0x00
 #define CAT_PARAM		0x01
-#define CAT_MONITOR		0x02
+#define CAT_MON			0x02
 #define CAT_AE			0x03
 #define CAT_WB			0x06
 #define CAT_EXIF		0x07
-#define CAT_FD			0x09
 #define CAT_LENS		0x0a
-#define CAT_CAPT_PARM		0x0b
-#define CAT_CAPT_CTRL		0x0c
-#define CAT_FLASH		0x0f	/* related to FW, revisions, booting */
+#define CAT_CAPTURE_PARAMETER	0x0b
+#define CAT_CAPTURE_CONTROL	0x0c
+#define CAT_FLASH		0x0f	/* related with FW, Verions, booting */
 
 /*
- * Category 0 - SYSTEM mode
- *
- * The SYSTEM mode in the M-5MOLS means area available to handle with the whole
- * & all-round system of sensor. It deals with version/interrupt/setting mode &
- * even sensor's status. Especially, the M-5MOLS sensor with ISP varies by
- * packaging & manufacturer, even the customer and project code. And the
- * function details may vary among them. The version information helps to
- * determine what methods shall be used in the driver.
+ * Category 0 - System
  *
- * There is many registers between customer version address and awb one. For
- * more specific contents, see definition if file m5mols.h.
+ * This category supports FW version, managing mode, even interrupt.
  */
-#define CAT0_VER_CUSTOMER	0x00	/* customer version */
-#define CAT0_VER_PROJECT	0x01	/* project version */
-#define CAT0_VER_FIRMWARE	0x02	/* Firmware version */
-#define CAT0_VER_HARDWARE	0x04	/* Hardware version */
-#define CAT0_VER_PARAMETER	0x06	/* Parameter version */
-#define CAT0_VER_AWB		0x08	/* Auto WB version */
-#define CAT0_VER_STRING		0x0a	/* string including M-5MOLS */
-#define CAT0_SYSMODE		0x0b	/* SYSTEM mode register */
-#define CAT0_STATUS		0x0c	/* SYSTEM mode status register */
-#define CAT0_INT_FACTOR		0x10	/* interrupt pending register */
-#define CAT0_INT_ENABLE		0x11	/* interrupt enable register */
-
-#define SYSTEM_VER_CUSTOMER	I2C_REG(CAT_SYSTEM, CAT0_VER_CUSTOMER, 1)
-#define SYSTEM_VER_PROJECT	I2C_REG(CAT_SYSTEM, CAT0_VER_PROJECT, 1)
-#define SYSTEM_VER_FIRMWARE	I2C_REG(CAT_SYSTEM, CAT0_VER_FIRMWARE, 2)
-#define SYSTEM_VER_HARDWARE	I2C_REG(CAT_SYSTEM, CAT0_VER_HARDWARE, 2)
-#define SYSTEM_VER_PARAMETER	I2C_REG(CAT_SYSTEM, CAT0_VER_PARAMETER, 2)
-#define SYSTEM_VER_AWB		I2C_REG(CAT_SYSTEM, CAT0_VER_AWB, 2)
-
-#define SYSTEM_SYSMODE		I2C_REG(CAT_SYSTEM, CAT0_SYSMODE, 1)
-#define REG_SYSINIT		0x00	/* SYSTEM mode */
-#define REG_PARAMETER		0x01	/* PARAMETER mode */
-#define REG_MONITOR		0x02	/* MONITOR mode */
-#define REG_CAPTURE		0x03	/* CAPTURE mode */
-
-#define SYSTEM_CMD(__cmd)	I2C_REG(CAT_SYSTEM, cmd, 1)
-#define SYSTEM_VER_STRING	I2C_REG(CAT_SYSTEM, CAT0_VER_STRING, 1)
-#define REG_SAMSUNG_ELECTRO	"SE"	/* Samsung Electro-Mechanics */
-#define REG_SAMSUNG_OPTICS	"OP"	/* Samsung Fiber-Optics */
-#define REG_SAMSUNG_TECHWIN	"TB"	/* Samsung Techwin */
-
-#define SYSTEM_INT_FACTOR	I2C_REG(CAT_SYSTEM, CAT0_INT_FACTOR, 1)
-#define SYSTEM_INT_ENABLE	I2C_REG(CAT_SYSTEM, CAT0_INT_ENABLE, 1)
-#define REG_INT_MODE		(1 << 0)
-#define REG_INT_AF		(1 << 1)
-#define REG_INT_ZOOM		(1 << 2)
-#define REG_INT_CAPTURE		(1 << 3)
-#define REG_INT_FRAMESYNC	(1 << 4)
-#define REG_INT_FD		(1 << 5)
-#define REG_INT_LENS_INIT	(1 << 6)
-#define REG_INT_SOUND		(1 << 7)
-#define REG_INT_MASK		0x0f
+#define CAT0_CUSTOMER_CODE	0x00
+#define CAT0_PJ_CODE		0x01
+#define CAT0_VERSION_FW_H	0x02
+#define CAT0_VERSION_FW_L	0x03
+#define CAT0_VERSION_HW_H	0x04
+#define CAT0_VERSION_HW_L	0x05
+#define CAT0_VERSION_PARM_H	0x06
+#define CAT0_VERSION_PARM_L	0x07
+#define CAT0_VERSION_AWB_H	0x08
+#define CAT0_VERSION_AWB_L	0x09
+#define CAT0_SYSMODE		0x0b
+#define CAT0_STATUS		0x0c
+#define CAT0_INT_QUEUE		0x0e
+#define CAT0_INT_FACTOR		0x10	/* ver 10bit: 0x10, other 0x1c */
+#define CAT0_INT_ENABLE		0x11	/* ver 10bit: 0x11, other 0x10  */
+#define CAT0_INT_ROOTEN		0x12
+#define CAT0_INT_INFO		0x18
 
 /*
- * category 1 - PARAMETER mode
+ * category 1 - Parameter mode
  *
- * This category supports function of camera features of M-5MOLS. It means we
- * can handle with preview(MONITOR) resolution size/frame per second/interface
- * between the sensor and the Application Processor/even the image effect.
+ * This category is dealing with almost camera vendor. In spite of that,
+ * It's a register to be able to detailed value for whole camera syste.
+ * The key parameter like a resolution, FPS, data interface connecting
+ * with Mobile AP, even effects.
  */
-#define CAT1_DATA_INTERFACE	0x00	/* interface between sensor and AP */
-#define CAT1_MONITOR_SIZE	0x01	/* resolution at the MONITOR mode */
-#define CAT1_MONITOR_FPS	0x02	/* frame per second at this mode */
-#define CAT1_EFFECT		0x0b	/* image effects */
-
-#define PARM_MON_SIZE		I2C_REG(CAT_PARAM, CAT1_MONITOR_SIZE, 1)
-
-#define PARM_MON_FPS		I2C_REG(CAT_PARAM, CAT1_MONITOR_FPS, 1)
-#define REG_FPS_30		0x02
-
-#define PARM_INTERFACE		I2C_REG(CAT_PARAM, CAT1_DATA_INTERFACE, 1)
-#define REG_INTERFACE_MIPI	0x02
-
-#define PARM_EFFECT		I2C_REG(CAT_PARAM, CAT1_EFFECT, 1)
-#define REG_EFFECT_OFF		0x00
-#define REG_EFFECT_NEGA		0x01
-#define REG_EFFECT_EMBOSS	0x06
-#define REG_EFFECT_OUTLINE	0x07
-#define REG_EFFECT_WATERCOLOR	0x08
+#define CAT1_DATA_INTERFACE	0x00
+#define CAT1_MONITOR_SIZE	0x01
+#define CAT1_MONITOR_FPS	0x02
+#define CAT1_EFFECT		0x0b
 
 /*
- * Category 2 - MONITOR mode
+ * Category 2 - Monitor mode
  *
- * The MONITOR mode is same as preview mode as we said. The M-5MOLS has another
- * mode named "Preview", but this preview mode is used at the case specific
- * vider-recording mode. This mmode supports only YUYV format. On the other
- * hand, the JPEG & RAW formats is supports by CAPTURE mode. And, there are
- * another options like zoom/color effect(different with effect in PARAMETER
- * mode)/anti hand shaking algorithm.
+ * This category supports only monitoring mode. The monitoring mode means,
+ * similar to preview. It supports like a YUYV format. At the capture mode,
+ * it is handled like a JPEG & RAW formats.
  */
-#define CAT2_ZOOM		0x01	/* set the zoom position & execute */
-#define CAT2_ZOOM_STEP		0x03	/* set the zoom step */
-#define CAT2_CFIXB		0x09	/* CB value for color effect */
-#define CAT2_CFIXR		0x0a	/* CR value for color effect */
-#define CAT2_COLOR_EFFECT	0x0b	/* set on/off of color effect */
-#define CAT2_CHROMA_LVL		0x0f	/* set chroma level */
-#define CAT2_CHROMA_EN		0x10	/* set on/off of choroma */
-#define CAT2_EDGE_LVL		0x11	/* set sharpness level */
-#define CAT2_EDGE_EN		0x12	/* set on/off sharpness */
-#define CAT2_TONE_CTL		0x25	/* set tone color(contrast) */
-
-#define MON_ZOOM		I2C_REG(CAT_MONITOR, CAT2_ZOOM, 1)
-
-#define MON_CFIXR		I2C_REG(CAT_MONITOR, CAT2_CFIXR, 1)
-#define MON_CFIXB		I2C_REG(CAT_MONITOR, CAT2_CFIXB, 1)
-#define REG_CFIXB_SEPIA		0xd8
-#define REG_CFIXR_SEPIA		0x18
-
-#define MON_EFFECT		I2C_REG(CAT_MONITOR, CAT2_COLOR_EFFECT, 1)
-#define REG_COLOR_EFFECT_OFF	0x00
-#define REG_COLOR_EFFECT_ON	0x01
-
-#define MON_CHROMA_EN		I2C_REG(CAT_MONITOR, CAT2_CHROMA_EN, 1)
-#define MON_CHROMA_LVL		I2C_REG(CAT_MONITOR, CAT2_CHROMA_LVL, 1)
-#define REG_CHROMA_OFF		0x00
-#define REG_CHROMA_ON		0x01
-
-#define MON_EDGE_EN		I2C_REG(CAT_MONITOR, CAT2_EDGE_EN, 1)
-#define MON_EDGE_LVL		I2C_REG(CAT_MONITOR, CAT2_EDGE_LVL, 1)
-#define REG_EDGE_OFF		0x00
-#define REG_EDGE_ON		0x01
-
-#define MON_TONE_CTL		I2C_REG(CAT_MONITOR, CAT2_TONE_CTL, 1)
+#define CAT2_ZOOM		0x01
+#define CAT2_ZOOM_POSITION	0x02
+#define CAT2_ZOOM_STEP		0x03
+#define CAT2_CFIXB		0x09
+#define CAT2_CFIXR		0x0a
+#define CAT2_COLOR_EFFECT	0x0b
+#define CAT2_CHROMA_LVL		0x0f
+#define CAT2_CHROMA_EN		0x10
 
 /*
  * Category 3 - Auto Exposure
  *
- * The M-5MOLS exposure capbility is detailed as which is similar to digital
- * camera. This category supports AE locking/various AE mode(range of exposure)
- * /ISO/flickering/EV bias/shutter/meteoring, and anything else. And the
- * maximum/minimum exposure gain value depending on M-5MOLS firmware, may be
- * different. So, this category also provide getting the max/min values. And,
- * each MONITOR and CAPTURE mode has each gain/shutter/max exposure values.
+ * Currently, it supports only gain value with monitor mode. This device
+ * is able to support Shutter, Gain(similar with Aperture), Flicker, at
+ * monitor mode & capture mode both.
  */
-#define CAT3_AE_LOCK		0x00	/* locking Auto exposure */
-#define CAT3_AE_MODE		0x01	/* set AE mode, mode means range */
-#define CAT3_ISO		0x05	/* set ISO */
-#define CAT3_EV_PRESET_MONITOR	0x0a	/* EV(scenemode) preset for MONITOR */
-#define CAT3_EV_PRESET_CAPTURE	0x0b	/* EV(scenemode) preset for CAPTURE */
-#define CAT3_MANUAL_GAIN_MON	0x12	/* meteoring value for the MONITOR */
-#define CAT3_MAX_GAIN_MON	0x1a	/* max gain value for the MONITOR */
-#define CAT3_MANUAL_GAIN_CAP	0x26	/* meteoring value for the CAPTURE */
-#define CAT3_AE_INDEX		0x38	/* AE index */
-
-#define AE_LOCK			I2C_REG(CAT_AE, CAT3_AE_LOCK, 1)
-#define REG_AE_UNLOCK		0x00
-#define REG_AE_LOCK		0x01
-
-#define AE_MODE			I2C_REG(CAT_AE, CAT3_AE_MODE, 1)
-#define REG_AE_OFF		0x00	/* AE off */
-#define REG_AE_ALL		0x01	/* calc AE in all block integral */
-#define REG_AE_CENTER		0x03	/* calc AE in center weighted */
-#define REG_AE_SPOT		0x06	/* calc AE in specific spot */
-
-#define AE_ISO			I2C_REG(CAT_AE, CAT3_ISO, 1)
-#define REG_ISO_AUTO		0x00
-#define REG_ISO_50		0x01
-#define REG_ISO_100		0x02
-#define REG_ISO_200		0x03
-#define REG_ISO_400		0x04
-#define REG_ISO_800		0x05
-
-#define AE_EV_PRESET_MONITOR	I2C_REG(CAT_AE, CAT3_EV_PRESET_MONITOR, 1)
-#define AE_EV_PRESET_CAPTURE	I2C_REG(CAT_AE, CAT3_EV_PRESET_CAPTURE, 1)
-#define REG_SCENE_NORMAL	0x00
-#define REG_SCENE_PORTRAIT	0x01
-#define REG_SCENE_LANDSCAPE	0x02
-#define REG_SCENE_SPORTS	0x03
-#define REG_SCENE_PARTY_INDOOR	0x04
-#define REG_SCENE_BEACH_SNOW	0x05
-#define REG_SCENE_SUNSET	0x06
-#define REG_SCENE_DAWN_DUSK	0x07
-#define REG_SCENE_FALL		0x08
-#define REG_SCENE_NIGHT		0x09
-#define REG_SCENE_AGAINST_LIGHT	0x0a
-#define REG_SCENE_FIRE		0x0b
-#define REG_SCENE_TEXT		0x0c
-#define REG_SCENE_CANDLE	0x0d
-
-#define AE_MAN_GAIN_MON		I2C_REG(CAT_AE, CAT3_MANUAL_GAIN_MON, 2)
-#define AE_MAX_GAIN_MON		I2C_REG(CAT_AE, CAT3_MAX_GAIN_MON, 2)
-#define AE_MAN_GAIN_CAP		I2C_REG(CAT_AE, CAT3_MANUAL_GAIN_CAP, 2)
-
-#define AE_INDEX		I2C_REG(CAT_AE, CAT3_AE_INDEX, 1)
-#define REG_AE_INDEX_20_NEG	0x00
-#define REG_AE_INDEX_15_NEG	0x01
-#define REG_AE_INDEX_10_NEG	0x02
-#define REG_AE_INDEX_05_NEG	0x03
-#define REG_AE_INDEX_00		0x04
-#define REG_AE_INDEX_05_POS	0x05
-#define REG_AE_INDEX_10_POS	0x06
-#define REG_AE_INDEX_15_POS	0x07
-#define REG_AE_INDEX_20_POS	0x08
+#define CAT3_AE_LOCK		0x00
+#define CAT3_AE_MODE		0x01
+#define CAT3_MANUAL_GAIN_MON	0x12	/* 2 bytes operations belows */
+#define CAT3_MANUAL_SHUT_MON	0x14
+#define CAT3_MAX_EXPOSURE_MON	0x16
+#define CAT3_MAX_EXPOSURE_CAP	0x18
+#define CAT3_MAX_GAIN_MON	0x1a
+#define CAT3_MAX_GAIN_CAP	0x1c
+#define CAT3_MANUAL_GAIN_CAP	0x26
+#define CAT3_MANUAL_SHUT_CAP	0x28
 
 /*
  * Category 6 - White Balance
  *
- * This category provide AWB locking/mode/preset/speed/gain bias, etc.
+ * Currently, it supports only auto white balance.
  */
-#define CAT6_AWB_LOCK		0x00	/* locking Auto Whitebalance */
-#define CAT6_AWB_MODE		0x02	/* set Auto or Manual */
-#define CAT6_AWB_MANUAL		0x03	/* set Manual(preset) value */
-
-#define AWB_LOCK		I2C_REG(CAT_WB, CAT6_AWB_LOCK, 1)
-#define REG_AWB_UNLOCK		0x00
-#define REG_AWB_LOCK		0x01
-
-#define AWB_MODE		I2C_REG(CAT_WB, CAT6_AWB_MODE, 1)
-#define REG_AWB_AUTO		0x01	/* AWB off */
-#define REG_AWB_PRESET		0x02	/* AWB preset */
-
-#define AWB_MANUAL		I2C_REG(CAT_WB, CAT6_AWB_MANUAL, 1)
-#define REG_AWB_INCANDESCENT	0x01
-#define REG_AWB_FLUORESCENT_1	0x02
-#define REG_AWB_FLUORESCENT_2	0x03
-#define REG_AWB_DAYLIGHT	0x04
-#define REG_AWB_CLOUDY		0x05
-#define REG_AWB_SHADE		0x06
-#define REG_AWB_HORIZON		0x07
-#define REG_AWB_LEDLIGHT	0x09
+#define CAT6_AWB_LOCK		0x00
+#define CAT6_AWB_MODE		0x02
+#define CAT6_AWB_MANUAL		0x03
 
 /*
- * Category 7 - EXIF information
+ * Category 7 - EXIF Information
  */
 #define CAT7_INFO_EXPTIME_NU	0x00
 #define CAT7_INFO_EXPTIME_DE	0x04
@@ -284,127 +130,35 @@
 #define CAT7_INFO_SDR		0x2c
 #define CAT7_INFO_QVAL		0x2e
 
-#define EXIF_INFO_EXPTIME_NU	I2C_REG(CAT_EXIF, CAT7_INFO_EXPTIME_NU, 4)
-#define EXIF_INFO_EXPTIME_DE	I2C_REG(CAT_EXIF, CAT7_INFO_EXPTIME_DE, 4)
-#define EXIF_INFO_TV_NU		I2C_REG(CAT_EXIF, CAT7_INFO_TV_NU, 4)
-#define EXIF_INFO_TV_DE		I2C_REG(CAT_EXIF, CAT7_INFO_TV_DE, 4)
-#define EXIF_INFO_AV_NU		I2C_REG(CAT_EXIF, CAT7_INFO_AV_NU, 4)
-#define EXIF_INFO_AV_DE		I2C_REG(CAT_EXIF, CAT7_INFO_AV_DE, 4)
-#define EXIF_INFO_BV_NU		I2C_REG(CAT_EXIF, CAT7_INFO_BV_NU, 4)
-#define EXIF_INFO_BV_DE		I2C_REG(CAT_EXIF, CAT7_INFO_BV_DE, 4)
-#define EXIF_INFO_EBV_NU	I2C_REG(CAT_EXIF, CAT7_INFO_EBV_NU, 4)
-#define EXIF_INFO_EBV_DE	I2C_REG(CAT_EXIF, CAT7_INFO_EBV_DE, 4)
-#define EXIF_INFO_ISO		I2C_REG(CAT_EXIF, CAT7_INFO_ISO, 2)
-#define EXIF_INFO_FLASH		I2C_REG(CAT_EXIF, CAT7_INFO_FLASH, 2)
-#define EXIF_INFO_SDR		I2C_REG(CAT_EXIF, CAT7_INFO_SDR, 2)
-#define EXIF_INFO_QVAL		I2C_REG(CAT_EXIF, CAT7_INFO_QVAL, 2)
-
-/*
- * Category 9 - Face Detection
- */
-#define CAT9_FD_CTL		0x00
-
-#define FD_CTL			I2C_REG(CAT_FD, CAT9_FD_CTL, 1)
-#define BIT_FD_EN		0
-#define BIT_FD_DRAW_FACE_FRAME	4
-#define BIT_FD_DRAW_SMILE_LVL	6
-#define REG_FD(shift)		(1 << shift)
-#define REG_FD_OFF		0x0
-
 /*
  * Category A - Lens Parameter
  */
+#define CATA_INIT_AF_FUNC	0x00
 #define CATA_AF_MODE		0x01
-#define CATA_AF_EXECUTE		0x02
+#define CATA_AF_EXCUTE		0x02
 #define CATA_AF_STATUS		0x03
 #define CATA_AF_VERSION		0x0a
 
-#define AF_MODE			I2C_REG(CAT_LENS, CATA_AF_MODE, 1)
-#define REG_AF_NORMAL		0x00	/* Normal AF, one time */
-#define REG_AF_MACRO		0x01	/* Macro AF, one time */
-#define REG_AF_POWEROFF		0x07
-
-#define AF_EXECUTE		I2C_REG(CAT_LENS, CATA_AF_EXECUTE, 1)
-#define REG_AF_STOP		0x00
-#define REG_AF_EXE_AUTO		0x01
-#define REG_AF_EXE_CAF		0x02
-
-#define AF_STATUS		I2C_REG(CAT_LENS, CATA_AF_STATUS, 1)
-#define REG_AF_FAIL		0x00
-#define REG_AF_SUCCESS		0x02
-#define REG_AF_IDLE		0x04
-#define REG_AF_BUSY		0x05
-
-#define AF_VERSION		I2C_REG(CAT_LENS, CATA_AF_VERSION, 1)
-
 /*
- * Category B - CAPTURE Parameter
+ * Category B - Capture Parameter
  */
 #define CATB_YUVOUT_MAIN	0x00
 #define CATB_MAIN_IMAGE_SIZE	0x01
-#define CATB_MCC_MODE		0x1d
-#define CATB_WDR_EN		0x2c
-#define CATB_LIGHT_CTRL		0x40
-#define CATB_FLASH_CTRL		0x41
-
-#define CAPP_YUVOUT_MAIN	I2C_REG(CAT_CAPT_PARM, CATB_YUVOUT_MAIN, 1)
-#define REG_YUV422		0x00
-#define REG_BAYER10		0x05
-#define REG_BAYER8		0x06
-#define REG_JPEG		0x10
-
-#define CAPP_MAIN_IMAGE_SIZE	I2C_REG(CAT_CAPT_PARM, CATB_MAIN_IMAGE_SIZE, 1)
-
-#define CAPP_MCC_MODE		I2C_REG(CAT_CAPT_PARM, CATB_MCC_MODE, 1)
-#define REG_MCC_OFF		0x00
-#define REG_MCC_NORMAL		0x01
-
-#define CAPP_WDR_EN		I2C_REG(CAT_CAPT_PARM, CATB_WDR_EN, 1)
-#define REG_WDR_OFF		0x00
-#define REG_WDR_ON		0x01
-#define REG_WDR_AUTO		0x02
-
-#define CAPP_LIGHT_CTRL		I2C_REG(CAT_CAPT_PARM, CATB_LIGHT_CTRL, 1)
-#define REG_LIGHT_OFF		0x00
-#define REG_LIGHT_ON		0x01
-#define REG_LIGHT_AUTO		0x02
-
-#define CAPP_FLASH_CTRL		I2C_REG(CAT_CAPT_PARM, CATB_FLASH_CTRL, 1)
-#define REG_FLASH_OFF		0x00
-#define REG_FLASH_ON		0x01
-#define REG_FLASH_AUTO		0x02
 
 /*
- * Category C - CAPTURE Control
+ * Category C - Capture Control
  */
-#define CATC_CAP_MODE		0x00
-#define CATC_CAP_SEL_FRAME	0x06	/* It determines Single or Multi */
+#define CATC_CAP_SEL_FRAME	0x06	/* It determines Single or Multi. */
 #define CATC_CAP_START		0x09
 #define CATC_CAP_IMAGE_SIZE	0x0d
 #define CATC_CAP_THUMB_SIZE	0x11
 
-#define CAPC_MODE		I2C_REG(CAT_CAPT_CTRL, CATC_CAP_MODE, 1)
-#define REG_CAP_NONE		0x00
-#define REG_CAP_ANTI_SHAKE	0x02
-
-#define CAPC_SEL_FRAME		I2C_REG(CAT_CAPT_CTRL, CATC_CAP_SEL_FRAME, 1)
-
-#define CAPC_START		I2C_REG(CAT_CAPT_CTRL, CATC_CAP_START, 1)
-#define REG_CAP_START_MAIN	0x01
-#define REG_CAP_START_THUMB	0x03
-
-#define CAPC_IMAGE_SIZE		I2C_REG(CAT_CAPT_CTRL, CATC_CAP_IMAGE_SIZE, 4)
-#define CAPC_THUMB_SIZE		I2C_REG(CAT_CAPT_CTRL, CATC_CAP_THUMB_SIZE, 4)
-
 /*
  * Category F - Flash
  *
- * This mode provides functions about internal flash stuff and system startup.
+ * This mode provides functions about internal Flash works and System startup.
  */
-#define CATF_CAM_START		0x12	/* It starts internal ARM core booting
+#define CATC_CAM_START		0x12	/* It start internal ARM core booting
 					 * after power-up */
 
-#define FLASH_CAM_START		I2C_REG(CAT_FLASH, CATF_CAM_START, 1)
-#define REG_START_ARM_BOOT	0x01
-
 #endif	/* M5MOLS_REG_H */