3 files changed, 161 insertions, 67 deletions

diff --git a/drivers/media/video/s5p-jpeg/jpeg-core.c b/drivers/media/video/s5p-jpeg/jpeg-core.c
index 1105a8749c8b..5a49c307f9c1 100644
--- a/drivers/media/video/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/video/s5p-jpeg/jpeg-core.c
@@ -32,10 +32,9 @@
 
 static struct s5p_jpeg_fmt formats_enc[] = {
         {
-                .name           = "YUV 4:2:0 planar, YCbCr",
-                .fourcc         = V4L2_PIX_FMT_YUV420,
-                .depth          = 12,
-                .colplanes      = 3,
+                .name           = "JPEG JFIF",
+                .fourcc         = V4L2_PIX_FMT_JPEG,
+                .colplanes      = 1,
                 .types          = MEM2MEM_CAPTURE,
         },
         {
@@ -43,7 +42,7 @@ static struct s5p_jpeg_fmt formats_enc[] = {
                 .fourcc         = V4L2_PIX_FMT_YUYV,
                 .depth          = 16,
                 .colplanes      = 1,
-                .types          = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
+                .types          = MEM2MEM_OUTPUT,
         },
         {
                 .name           = "RGB565",
@@ -203,6 +202,16 @@ static const unsigned char hactblg0[162] = {
         0xf9, 0xfa
 };
 
+static inline struct s5p_jpeg_ctx *ctrl_to_ctx(struct v4l2_ctrl *c)
+{
+        return container_of(c->handler, struct s5p_jpeg_ctx, ctrl_handler);
+}
+
+static inline struct s5p_jpeg_ctx *fh_to_ctx(struct v4l2_fh *fh)
+{
+        return container_of(fh, struct s5p_jpeg_ctx, fh);
+}
+
 static inline void jpeg_set_qtbl(void __iomem *regs, const unsigned char *qtbl,
                    unsigned long tab, int len)
 {
@@ -269,6 +278,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
                       struct vb2_queue *dst_vq);
 static struct s5p_jpeg_fmt *s5p_jpeg_find_format(unsigned int mode,
                                                  __u32 pixelformat);
+static int s5p_jpeg_controls_create(struct s5p_jpeg_ctx *ctx);
 
 static int s5p_jpeg_open(struct file *file)
 {
@@ -276,12 +286,18 @@ static int s5p_jpeg_open(struct file *file)
         struct video_device *vfd = video_devdata(file);
         struct s5p_jpeg_ctx *ctx;
         struct s5p_jpeg_fmt *out_fmt;
+        int ret = 0;
 
         ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
         if (!ctx)
                 return -ENOMEM;
 
-        file->private_data = ctx;
+        v4l2_fh_init(&ctx->fh, vfd);
+        /* Use separate control handler per file handle */
+        ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+        file->private_data = &ctx->fh;
+        v4l2_fh_add(&ctx->fh);
+
         ctx->jpeg = jpeg;
         if (vfd == jpeg->vfd_encoder) {
                 ctx->mode = S5P_JPEG_ENCODE;
@@ -291,24 +307,35 @@ static int s5p_jpeg_open(struct file *file)
                 out_fmt = s5p_jpeg_find_format(ctx->mode, V4L2_PIX_FMT_JPEG);
         }
 
+        ret = s5p_jpeg_controls_create(ctx);
+        if (ret < 0)
+                goto error;
+
         ctx->m2m_ctx = v4l2_m2m_ctx_init(jpeg->m2m_dev, ctx, queue_init);
         if (IS_ERR(ctx->m2m_ctx)) {
-                int err = PTR_ERR(ctx->m2m_ctx);
-                kfree(ctx);
-                return err;
+                ret = PTR_ERR(ctx->m2m_ctx);
+                goto error;
         }
 
         ctx->out_q.fmt = out_fmt;
         ctx->cap_q.fmt = s5p_jpeg_find_format(ctx->mode, V4L2_PIX_FMT_YUYV);
-
         return 0;
+
+error:
+        v4l2_fh_del(&ctx->fh);
+        v4l2_fh_exit(&ctx->fh);
+        kfree(ctx);
+        return ret;
 }
 
 static int s5p_jpeg_release(struct file *file)
 {
-        struct s5p_jpeg_ctx *ctx = file->private_data;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(file->private_data);
 
         v4l2_m2m_ctx_release(ctx->m2m_ctx);
+        v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+        v4l2_fh_del(&ctx->fh);
+        v4l2_fh_exit(&ctx->fh);
         kfree(ctx);
 
         return 0;
@@ -317,14 +344,14 @@ static int s5p_jpeg_release(struct file *file)
 static unsigned int s5p_jpeg_poll(struct file *file,
                                  struct poll_table_struct *wait)
 {
-        struct s5p_jpeg_ctx *ctx = file->private_data;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(file->private_data);
 
         return v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
 }
 
 static int s5p_jpeg_mmap(struct file *file, struct vm_area_struct *vma)
 {
-        struct s5p_jpeg_ctx *ctx = file->private_data;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(file->private_data);
 
         return v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
 }
@@ -448,7 +475,7 @@ static bool s5p_jpeg_parse_hdr(struct s5p_jpeg_q_data *result,
 static int s5p_jpeg_querycap(struct file *file, void *priv,
                            struct v4l2_capability *cap)
 {
-        struct s5p_jpeg_ctx *ctx = priv;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
         if (ctx->mode == S5P_JPEG_ENCODE) {
                 strlcpy(cap->driver, S5P_JPEG_M2M_NAME " encoder",
@@ -497,9 +524,7 @@ static int enum_fmt(struct s5p_jpeg_fmt *formats, int n,
 static int s5p_jpeg_enum_fmt_vid_cap(struct file *file, void *priv,
                                    struct v4l2_fmtdesc *f)
 {
-        struct s5p_jpeg_ctx *ctx;
-
-        ctx = priv;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
         if (ctx->mode == S5P_JPEG_ENCODE)
                 return enum_fmt(formats_enc, NUM_FORMATS_ENC, f,
@@ -511,9 +536,7 @@ static int s5p_jpeg_enum_fmt_vid_cap(struct file *file, void *priv,
 static int s5p_jpeg_enum_fmt_vid_out(struct file *file, void *priv,
                                    struct v4l2_fmtdesc *f)
 {
-        struct s5p_jpeg_ctx *ctx;
-
-        ctx = priv;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
         if (ctx->mode == S5P_JPEG_ENCODE)
                 return enum_fmt(formats_enc, NUM_FORMATS_ENC, f,
@@ -538,7 +561,7 @@ static int s5p_jpeg_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
         struct vb2_queue *vq;
         struct s5p_jpeg_q_data *q_data = NULL;
         struct v4l2_pix_format *pix = &f->fmt.pix;
-        struct s5p_jpeg_ctx *ct = priv;
+        struct s5p_jpeg_ctx *ct = fh_to_ctx(priv);
 
         vq = v4l2_m2m_get_vq(ct->m2m_ctx, f->type);
         if (!vq)
@@ -659,8 +682,8 @@ static int vidioc_try_fmt(struct v4l2_format *f, struct s5p_jpeg_fmt *fmt,
 static int s5p_jpeg_try_fmt_vid_cap(struct file *file, void *priv,
                                   struct v4l2_format *f)
 {
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
         struct s5p_jpeg_fmt *fmt;
-        struct s5p_jpeg_ctx *ctx = priv;
 
         fmt = s5p_jpeg_find_format(ctx->mode, f->fmt.pix.pixelformat);
         if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) {
@@ -676,8 +699,8 @@ static int s5p_jpeg_try_fmt_vid_cap(struct file *file, void *priv,
 static int s5p_jpeg_try_fmt_vid_out(struct file *file, void *priv,
                                   struct v4l2_format *f)
 {
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
         struct s5p_jpeg_fmt *fmt;
-        struct s5p_jpeg_ctx *ctx = priv;
 
         fmt = s5p_jpeg_find_format(ctx->mode, f->fmt.pix.pixelformat);
         if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) {
@@ -728,7 +751,7 @@ static int s5p_jpeg_s_fmt_vid_cap(struct file *file, void *priv,
         if (ret)
                 return ret;
 
-        return s5p_jpeg_s_fmt(priv, f);
+        return s5p_jpeg_s_fmt(fh_to_ctx(priv), f);
 }
 
 static int s5p_jpeg_s_fmt_vid_out(struct file *file, void *priv,
@@ -740,13 +763,13 @@ static int s5p_jpeg_s_fmt_vid_out(struct file *file, void *priv,
         if (ret)
                 return ret;
 
-        return s5p_jpeg_s_fmt(priv, f);
+        return s5p_jpeg_s_fmt(fh_to_ctx(priv), f);
 }
 
 static int s5p_jpeg_reqbufs(struct file *file, void *priv,
                           struct v4l2_requestbuffers *reqbufs)
 {
-        struct s5p_jpeg_ctx *ctx = priv;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
         return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
 }
@@ -754,14 +777,14 @@ static int s5p_jpeg_reqbufs(struct file *file, void *priv,
 static int s5p_jpeg_querybuf(struct file *file, void *priv,
                            struct v4l2_buffer *buf)
 {
-        struct s5p_jpeg_ctx *ctx = priv;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
         return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
 }
 
 static int s5p_jpeg_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 {
-        struct s5p_jpeg_ctx *ctx = priv;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
         return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
 }
@@ -769,7 +792,7 @@ static int s5p_jpeg_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 static int s5p_jpeg_dqbuf(struct file *file, void *priv,
                           struct v4l2_buffer *buf)
 {
-        struct s5p_jpeg_ctx *ctx = priv;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
         return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
 }
@@ -777,7 +800,7 @@ static int s5p_jpeg_dqbuf(struct file *file, void *priv,
 static int s5p_jpeg_streamon(struct file *file, void *priv,
                            enum v4l2_buf_type type)
 {
-        struct s5p_jpeg_ctx *ctx = priv;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
         return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
 }
@@ -785,7 +808,7 @@ static int s5p_jpeg_streamon(struct file *file, void *priv,
 static int s5p_jpeg_streamoff(struct file *file, void *priv,
                             enum v4l2_buf_type type)
 {
-        struct s5p_jpeg_ctx *ctx = priv;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
         return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
 }
@@ -793,7 +816,7 @@ static int s5p_jpeg_streamoff(struct file *file, void *priv,
 int s5p_jpeg_g_selection(struct file *file, void *priv,
                          struct v4l2_selection *s)
 {
-        struct s5p_jpeg_ctx *ctx = priv;
+        struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
         if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
             s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
@@ -822,33 +845,89 @@ int s5p_jpeg_g_selection(struct file *file, void *priv,
         return 0;
 }
 
-static int s5p_jpeg_g_jpegcomp(struct file *file, void *priv,
-                               struct v4l2_jpegcompression *compr)
+/*
+ * V4L2 controls
+ */
+
+static int s5p_jpeg_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
-        struct s5p_jpeg_ctx *ctx = priv;
+        struct s5p_jpeg_ctx *ctx = ctrl_to_ctx(ctrl);
+        struct s5p_jpeg *jpeg = ctx->jpeg;
+        unsigned long flags;
 
-        if (ctx->mode == S5P_JPEG_DECODE)
-                return -ENOTTY;
+        switch (ctrl->id) {
+        case V4L2_CID_JPEG_CHROMA_SUBSAMPLING:
+                spin_lock_irqsave(&jpeg->slock, flags);
 
-        memset(compr, 0, sizeof(*compr));
-        compr->quality = ctx->compr_quality;
+                WARN_ON(ctx->subsampling > S5P_SUBSAMPLING_MODE_GRAY);
+                if (ctx->subsampling > 2)
+                        ctrl->val = V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY;
+                else
+                        ctrl->val = ctx->subsampling;
+                spin_unlock_irqrestore(&jpeg->slock, flags);
+                break;
+        }
 
         return 0;
 }
 
-static int s5p_jpeg_s_jpegcomp(struct file *file, void *priv,
-                               struct v4l2_jpegcompression *compr)
+static int s5p_jpeg_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-        struct s5p_jpeg_ctx *ctx = priv;
+        struct s5p_jpeg_ctx *ctx = ctrl_to_ctx(ctrl);
+        unsigned long flags;
 
-        if (ctx->mode == S5P_JPEG_DECODE)
-                return -ENOTTY;
+        spin_lock_irqsave(&ctx->jpeg->slock, flags);
 
-        compr->quality = clamp(compr->quality, S5P_JPEG_COMPR_QUAL_BEST,
-                               S5P_JPEG_COMPR_QUAL_WORST);
+        switch (ctrl->id) {
+        case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+                ctx->compr_quality = S5P_JPEG_COMPR_QUAL_WORST - ctrl->val;
+                break;
+        case V4L2_CID_JPEG_RESTART_INTERVAL:
+                ctx->restart_interval = ctrl->val;
+                break;
+        case V4L2_CID_JPEG_CHROMA_SUBSAMPLING:
+                ctx->subsampling = ctrl->val;
+                break;
+        }
 
-        ctx->compr_quality = S5P_JPEG_COMPR_QUAL_WORST - compr->quality;
+        spin_unlock_irqrestore(&ctx->jpeg->slock, flags);
+        return 0;
+}
+
+static const struct v4l2_ctrl_ops s5p_jpeg_ctrl_ops = {
+        .g_volatile_ctrl        = s5p_jpeg_g_volatile_ctrl,
+        .s_ctrl                 = s5p_jpeg_s_ctrl,
+};
 
+static int s5p_jpeg_controls_create(struct s5p_jpeg_ctx *ctx)
+{
+        unsigned int mask = ~0x27; /* 444, 422, 420, GRAY */
+        struct v4l2_ctrl *ctrl;
+
+        v4l2_ctrl_handler_init(&ctx->ctrl_handler, 3);
+
+        if (ctx->mode == S5P_JPEG_ENCODE) {
+                v4l2_ctrl_new_std(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
+                                  V4L2_CID_JPEG_COMPRESSION_QUALITY,
+                                  0, 3, 1, 3);
+
+                v4l2_ctrl_new_std(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
+                                  V4L2_CID_JPEG_RESTART_INTERVAL,
+                                  0, 3, 0xffff, 0);
+                mask = ~0x06; /* 422, 420 */
+        }
+
+        ctrl = v4l2_ctrl_new_std_menu(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
+                                      V4L2_CID_JPEG_CHROMA_SUBSAMPLING,
+                                      V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY, mask,
+                                      V4L2_JPEG_CHROMA_SUBSAMPLING_422);
+
+        if (ctx->ctrl_handler.error)
+                return ctx->ctrl_handler.error;
+
+        if (ctx->mode == S5P_JPEG_DECODE)
+                ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE |
+                        V4L2_CTRL_FLAG_READ_ONLY;
         return 0;
 }
 
@@ -877,9 +956,6 @@ static const struct v4l2_ioctl_ops s5p_jpeg_ioctl_ops = {
         .vidioc_streamoff               = s5p_jpeg_streamoff,
 
         .vidioc_g_selection             = s5p_jpeg_g_selection,
-
-        .vidioc_g_jpegcomp              = s5p_jpeg_g_jpegcomp,
-        .vidioc_s_jpegcomp              = s5p_jpeg_s_jpegcomp,
 };
 
 /*
@@ -908,13 +984,8 @@ static void s5p_jpeg_device_run(void *priv)
                         jpeg_input_raw_mode(jpeg->regs, S5P_JPEG_RAW_IN_565);
                 else
                         jpeg_input_raw_mode(jpeg->regs, S5P_JPEG_RAW_IN_422);
-                if (ctx->cap_q.fmt->fourcc == V4L2_PIX_FMT_YUYV)
-                        jpeg_subsampling_mode(jpeg->regs,
-                                              S5P_JPEG_SUBSAMPLING_422);
-                else
-                        jpeg_subsampling_mode(jpeg->regs,
-                                              S5P_JPEG_SUBSAMPLING_420);
-                jpeg_dri(jpeg->regs, 0);
+                jpeg_subsampling_mode(jpeg->regs, ctx->subsampling);
+                jpeg_dri(jpeg->regs, ctx->restart_interval);
                 jpeg_x(jpeg->regs, ctx->out_q.w);
                 jpeg_y(jpeg->regs, ctx->out_q.h);
                 jpeg_imgadr(jpeg->regs, src_addr);
@@ -953,14 +1024,18 @@ static void s5p_jpeg_device_run(void *priv)
                 jpeg_htbl_dc(jpeg->regs, 2);
                 jpeg_htbl_ac(jpeg->regs, 3);
                 jpeg_htbl_dc(jpeg->regs, 3);
-        } else {
+        } else { /* S5P_JPEG_DECODE */
                 jpeg_rst_int_enable(jpeg->regs, true);
                 jpeg_data_num_int_enable(jpeg->regs, true);
                 jpeg_final_mcu_num_int_enable(jpeg->regs, true);
-                jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_422);
+                if (ctx->cap_q.fmt->fourcc == V4L2_PIX_FMT_YUYV)
+                        jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_422);
+                else
+                        jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_420);
                 jpeg_jpgadr(jpeg->regs, src_addr);
                 jpeg_imgadr(jpeg->regs, dst_addr);
         }
+
         jpeg_start(jpeg->regs);
 }
 
@@ -1162,6 +1237,8 @@ static irqreturn_t s5p_jpeg_irq(int irq, void *dev_id)
         bool timer_elapsed = false;
         bool op_completed = false;
 
+        spin_lock(&jpeg->slock);
+
         curr_ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
 
         src_buf = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
@@ -1192,6 +1269,9 @@ static irqreturn_t s5p_jpeg_irq(int irq, void *dev_id)
         v4l2_m2m_buf_done(dst_buf, state);
         v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->m2m_ctx);
 
+        curr_ctx->subsampling = jpeg_get_subsampling_mode(jpeg->regs);
+        spin_unlock(&jpeg->slock);
+
         jpeg_clear_int(jpeg->regs);
 
         return IRQ_HANDLED;
@@ -1215,6 +1295,7 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
                 return -ENOMEM;
 
         mutex_init(&jpeg->lock);
+        spin_lock_init(&jpeg->slock);
         jpeg->dev = &pdev->dev;
 
         /* memory-mapped registers */
diff --git a/drivers/media/video/s5p-jpeg/jpeg-core.h b/drivers/media/video/s5p-jpeg/jpeg-core.h
index facad6114f5e..38d7367f7a6d 100644
--- a/drivers/media/video/s5p-jpeg/jpeg-core.h
+++ b/drivers/media/video/s5p-jpeg/jpeg-core.h
@@ -14,6 +14,8 @@
 #define JPEG_CORE_H_
 
 #include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ctrls.h>
 
 #define S5P_JPEG_M2M_NAME               "s5p-jpeg"
 
@@ -47,6 +49,7 @@
 /**
  * struct s5p_jpeg - JPEG IP abstraction
  * @lock:               the mutex protecting this structure
+ * @slock:              spinlock protecting the device contexts
  * @v4l2_dev:           v4l2 device for mem2mem mode
  * @vfd_encoder:        video device node for encoder mem2mem mode
  * @vfd_decoder:        video device node for decoder mem2mem mode
@@ -60,6 +63,7 @@
  */
 struct s5p_jpeg {
         struct mutex            lock;
+        struct spinlock         slock;
 
         struct v4l2_device      v4l2_dev;
         struct video_device     *vfd_encoder;
@@ -117,15 +121,20 @@ struct s5p_jpeg_q_data {
  * @out_q:              source (output) queue information
  * @cap_fmt:            destination (capture) queue queue information
  * @hdr_parsed:         set if header has been parsed during decompression
+ * @ctrl_handler:       controls handler
  */
 struct s5p_jpeg_ctx {
         struct s5p_jpeg         *jpeg;
         unsigned int            mode;
-        unsigned int            compr_quality;
+        unsigned short          compr_quality;
+        unsigned short          restart_interval;
+        unsigned short          subsampling;
         struct v4l2_m2m_ctx     *m2m_ctx;
         struct s5p_jpeg_q_data  out_q;
         struct s5p_jpeg_q_data  cap_q;
+        struct v4l2_fh          fh;
         bool                    hdr_parsed;
+        struct v4l2_ctrl_handler ctrl_handler;
 };
 
 /**
diff --git a/drivers/media/video/s5p-jpeg/jpeg-hw.h b/drivers/media/video/s5p-jpeg/jpeg-hw.h
index e10c744e9f23..f12f0fdbde7c 100644
--- a/drivers/media/video/s5p-jpeg/jpeg-hw.h
+++ b/drivers/media/video/s5p-jpeg/jpeg-hw.h
@@ -13,6 +13,7 @@
 #define JPEG_HW_H_
 
 #include <linux/io.h>
+#include <linux/videodev2.h>
 
 #include "jpeg-hw.h"
 #include "jpeg-regs.h"
@@ -25,8 +26,6 @@
 #define S5P_JPEG_DECODE                 1
 #define S5P_JPEG_RAW_IN_565             0
 #define S5P_JPEG_RAW_IN_422             1
-#define S5P_JPEG_SUBSAMPLING_422        0
-#define S5P_JPEG_SUBSAMPLING_420        1
 #define S5P_JPEG_RAW_OUT_422            0
 #define S5P_JPEG_RAW_OUT_420            1
 
@@ -91,21 +90,26 @@ static inline void jpeg_proc_mode(void __iomem *regs, unsigned long mode)
         writel(reg, regs + S5P_JPGMOD);
 }
 
-static inline void jpeg_subsampling_mode(void __iomem *regs, unsigned long mode)
+static inline void jpeg_subsampling_mode(void __iomem *regs, unsigned int mode)
 {
         unsigned long reg, m;
 
-        m = S5P_SUBSAMPLING_MODE_422;
-        if (mode == S5P_JPEG_SUBSAMPLING_422)
-                m = S5P_SUBSAMPLING_MODE_422;
-        else if (mode == S5P_JPEG_SUBSAMPLING_420)
+        if (mode == V4L2_JPEG_CHROMA_SUBSAMPLING_420)
                 m = S5P_SUBSAMPLING_MODE_420;
+        else
+                m = S5P_SUBSAMPLING_MODE_422;
+
         reg = readl(regs + S5P_JPGMOD);
         reg &= ~S5P_SUBSAMPLING_MODE_MASK;
         reg |= m;
         writel(reg, regs + S5P_JPGMOD);
 }
 
+static inline unsigned int jpeg_get_subsampling_mode(void __iomem *regs)
+{
+        return readl(regs + S5P_JPGMOD) & S5P_SUBSAMPLING_MODE_MASK;
+}
+
 static inline void jpeg_dri(void __iomem *regs, unsigned int dri)
 {
         unsigned long reg;