170 files changed, 8074 insertions, 3212 deletions

diff --git a/Documentation/DocBook/media/v4l/compat.xml b/Documentation/DocBook/media/v4l/compat.xml
index c6ae4c9d0e0c..4fdf6b562d1c 100644
--- a/Documentation/DocBook/media/v4l/compat.xml
+++ b/Documentation/DocBook/media/v4l/compat.xml
@@ -2582,6 +2582,10 @@ ioctls.</para>
         <listitem>
           <para>Support for frequency band enumeration: &VIDIOC-ENUM-FREQ-BANDS; ioctl.</para>
         </listitem>
+        <listitem>
+          <para>Vendor and device specific media bus pixel formats.
+            <xref linkend="v4l2-mbus-vendor-spec-fmts" />.</para>
+        </listitem>
       </itemizedlist>
     </section>
 
diff --git a/Documentation/DocBook/media/v4l/controls.xml b/Documentation/DocBook/media/v4l/controls.xml
index 272a5f718509..7fe5be1d3bbb 100644
--- a/Documentation/DocBook/media/v4l/controls.xml
+++ b/Documentation/DocBook/media/v4l/controls.xml
@@ -1586,7 +1586,6 @@ frame counter of the frame that is currently displayed (decoded). This value is
 the decoder is started.</entry>
               </row>
 
-
               <row><entry></entry></row>
               <row>
                 <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_DECODER_SLICE_INTERFACE</constant>&nbsp;</entry>
@@ -2270,6 +2269,14 @@ Applicable to the MPEG1, MPEG2, MPEG4 encoders.</entry>
               </row>
 
               <row><entry></entry></row>
+              <row id="v4l2-mpeg-video-vbv-delay">
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_VBV_DELAY</constant>&nbsp;</entry>
+                <entry>integer</entry>
+              </row><row><entry spanname="descr">Sets the initial delay in milliseconds for
+VBV buffer control.</entry>
+              </row>
+
+              <row><entry></entry></row>
               <row>
                 <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_CPB_SIZE</constant>&nbsp;</entry>
                 <entry>integer</entry>
@@ -2334,6 +2341,265 @@ Applicable to the MPEG4 decoder.</entry>
               </row><row><entry spanname="descr">vop_time_increment value for MPEG4. Applicable to the MPEG4 encoder.</entry>
               </row>
 
+              <row><entry></entry></row>
+              <row>
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_SEI_FRAME_PACKING</constant>&nbsp;</entry>
+                <entry>boolean</entry>
+              </row>
+              <row><entry spanname="descr">Enable generation of frame packing supplemental enhancement information in the encoded bitstream.
+The frame packing SEI message contains the arrangement of L and R planes for 3D viewing. Applicable to the H264 encoder.</entry>
+              </row>
+
+              <row><entry></entry></row>
+              <row>
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_SEI_FP_CURRENT_FRAME_0</constant>&nbsp;</entry>
+                <entry>boolean</entry>
+              </row>
+              <row><entry spanname="descr">Sets current frame as frame0 in frame packing SEI.
+Applicable to the H264 encoder.</entry>
+              </row>
+
+              <row><entry></entry></row>
+              <row id="v4l2-mpeg-video-h264-sei-fp-arrangement-type">
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE</constant>&nbsp;</entry>
+                <entry>enum&nbsp;v4l2_mpeg_video_h264_sei_fp_arrangement_type</entry>
+              </row>
+              <row><entry spanname="descr">Frame packing arrangement type for H264 SEI.
+Applicable to the H264 encoder.
+Possible values are:</entry>
+              </row>
+              <row>
+                <entrytbl spanname="descr" cols="2">
+                  <tbody valign="top">
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_CHEKERBOARD</constant>&nbsp;</entry>
+                      <entry>Pixels are alternatively from L and R.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_COLUMN</constant>&nbsp;</entry>
+                      <entry>L and R are interlaced by column.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_ROW</constant>&nbsp;</entry>
+                      <entry>L and R are interlaced by row.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_SIDE_BY_SIDE</constant>&nbsp;</entry>
+                      <entry>L is on the left, R on the right.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_TOP_BOTTOM</constant>&nbsp;</entry>
+                      <entry>L is on top, R on bottom.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_TEMPORAL</constant>&nbsp;</entry>
+                      <entry>One view per frame.</entry>
+                    </row>
+                  </tbody>
+                </entrytbl>
+              </row>
+
+              <row><entry></entry></row>
+              <row>
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_FMO</constant>&nbsp;</entry>
+                <entry>boolean</entry>
+              </row>
+              <row><entry spanname="descr">Enables flexible macroblock ordering in the encoded bitstream. It is a technique
+used for restructuring the ordering of macroblocks in pictures. Applicable to the H264 encoder.</entry>
+              </row>
+
+              <row><entry></entry></row>
+              <row id="v4l2-mpeg-video-h264-fmo-map-type">
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_FMO_MAP_TYPE</constant>&nbsp;</entry>
+                <entry>enum&nbsp;v4l2_mpeg_video_h264_fmo_map_type</entry>
+              </row>
+              <row><entry spanname="descr">When using FMO, the map type divides the image in different scan patterns of macroblocks.
+Applicable to the H264 encoder.
+Possible values are:</entry>
+              </row>
+              <row>
+                <entrytbl spanname="descr" cols="2">
+                  <tbody valign="top">
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_INTERLEAVED_SLICES</constant>&nbsp;</entry>
+                      <entry>Slices are interleaved one after other with macroblocks in run length order.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_SCATTERED_SLICES</constant>&nbsp;</entry>
+                      <entry>Scatters the macroblocks based on a mathematical function known to both encoder and decoder.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_FOREGROUND_WITH_LEFT_OVER</constant>&nbsp;</entry>
+                      <entry>Macroblocks arranged in rectangular areas or regions of interest.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_BOX_OUT</constant>&nbsp;</entry>
+                      <entry>Slice groups grow in a cyclic way from centre to outwards.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_RASTER_SCAN</constant>&nbsp;</entry>
+                      <entry>Slice groups grow in raster scan pattern from left to right.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_WIPE_SCAN</constant>&nbsp;</entry>
+                      <entry>Slice groups grow in wipe scan pattern from top to bottom.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_EXPLICIT</constant>&nbsp;</entry>
+                      <entry>User defined map type.</entry>
+                    </row>
+                  </tbody>
+                </entrytbl>
+              </row>
+
+              <row><entry></entry></row>
+              <row>
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_FMO_SLICE_GROUP</constant>&nbsp;</entry>
+                <entry>integer</entry>
+              </row>
+              <row><entry spanname="descr">Number of slice groups in FMO.
+Applicable to the H264 encoder.</entry>
+              </row>
+
+              <row><entry></entry></row>
+              <row id="v4l2-mpeg-video-h264-fmo-change-direction">
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_FMO_CHANGE_DIRECTION</constant>&nbsp;</entry>
+                <entry>enum&nbsp;v4l2_mpeg_video_h264_fmo_change_dir</entry>
+              </row>
+              <row><entry spanname="descr">Specifies a direction of the slice group change for raster and wipe maps.
+Applicable to the H264 encoder.
+Possible values are:</entry>
+              </row>
+              <row>
+                <entrytbl spanname="descr" cols="2">
+                  <tbody valign="top">
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_FMO_CHANGE_DIR_RIGHT</constant>&nbsp;</entry>
+                      <entry>Raster scan or wipe right.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_FMO_CHANGE_DIR_LEFT</constant>&nbsp;</entry>
+                      <entry>Reverse raster scan or wipe left.</entry>
+                    </row>
+                  </tbody>
+                </entrytbl>
+              </row>
+
+              <row><entry></entry></row>
+              <row>
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_FMO_CHANGE_RATE</constant>&nbsp;</entry>
+                <entry>integer</entry>
+              </row>
+              <row><entry spanname="descr">Specifies the size of the first slice group for raster and wipe map.
+Applicable to the H264 encoder.</entry>
+              </row>
+
+              <row><entry></entry></row>
+              <row>
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_FMO_RUN_LENGTH</constant>&nbsp;</entry>
+                <entry>integer</entry>
+              </row>
+              <row><entry spanname="descr">Specifies the number of consecutive macroblocks for the interleaved map.
+Applicable to the H264 encoder.</entry>
+              </row>
+
+              <row><entry></entry></row>
+              <row>
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_ASO</constant>&nbsp;</entry>
+                <entry>boolean</entry>
+              </row>
+              <row><entry spanname="descr">Enables arbitrary slice ordering in encoded bitstream.
+Applicable to the H264 encoder.</entry>
+              </row>
+
+              <row><entry></entry></row>
+              <row>
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_ASO_SLICE_ORDER</constant>&nbsp;</entry>
+                <entry>integer</entry>
+              </row><row><entry spanname="descr">Specifies the slice order in ASO. Applicable to the H264 encoder.
+The supplied 32-bit integer is interpreted as follows (bit
+0 = least significant bit):</entry>
+              </row>
+              <row>
+                <entrytbl spanname="descr" cols="2">
+                  <tbody valign="top">
+                    <row>
+                      <entry>Bit 0:15</entry>
+                      <entry>Slice ID</entry>
+                    </row>
+                    <row>
+                      <entry>Bit 16:32</entry>
+                      <entry>Slice position or order</entry>
+                    </row>
+                  </tbody>
+                </entrytbl>
+              </row>
+
+              <row><entry></entry></row>
+              <row>
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING</constant>&nbsp;</entry>
+                <entry>boolean</entry>
+              </row>
+              <row><entry spanname="descr">Enables H264 hierarchical coding.
+Applicable to the H264 encoder.</entry>
+              </row>
+
+              <row><entry></entry></row>
+              <row id="v4l2-mpeg-video-h264-hierarchical-coding-type">
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_TYPE</constant>&nbsp;</entry>
+                <entry>enum&nbsp;v4l2_mpeg_video_h264_hierarchical_coding_type</entry>
+              </row>
+              <row><entry spanname="descr">Specifies the hierarchical coding type.
+Applicable to the H264 encoder.
+Possible values are:</entry>
+              </row>
+              <row>
+                <entrytbl spanname="descr" cols="2">
+                  <tbody valign="top">
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_HIERARCHICAL_CODING_B</constant>&nbsp;</entry>
+                      <entry>Hierarchical B coding.</entry>
+                    </row>
+                    <row>
+                      <entry><constant>V4L2_MPEG_VIDEO_H264_HIERARCHICAL_CODING_P</constant>&nbsp;</entry>
+                      <entry>Hierarchical P coding.</entry>
+                    </row>
+                  </tbody>
+                </entrytbl>
+              </row>
+
+              <row><entry></entry></row>
+              <row>
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_LAYER</constant>&nbsp;</entry>
+                <entry>integer</entry>
+              </row>
+              <row><entry spanname="descr">Specifies the number of hierarchical coding layers.
+Applicable to the H264 encoder.</entry>
+              </row>
+
+              <row><entry></entry></row>
+              <row>
+                <entry spanname="id"><constant>V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_LAYER_QP</constant>&nbsp;</entry>
+                <entry>integer</entry>
+              </row><row><entry spanname="descr">Specifies a user defined QP for each layer. Applicable to the H264 encoder.
+The supplied 32-bit integer is interpreted as follows (bit
+0 = least significant bit):</entry>
+              </row>
+              <row>
+                <entrytbl spanname="descr" cols="2">
+                  <tbody valign="top">
+                    <row>
+                      <entry>Bit 0:15</entry>
+                      <entry>QP value</entry>
+                    </row>
+                    <row>
+                      <entry>Bit 16:32</entry>
+                      <entry>Layer number</entry>
+                    </row>
+                  </tbody>
+                </entrytbl>
+              </row>
+
             </tbody>
           </tgroup>
         </table>
@@ -4267,6 +4533,16 @@ interface and may change in the future.</para>
             pixels / second.
             </entry>
           </row>
+          <row>
+            <entry spanname="id"><constant>V4L2_CID_TEST_PATTERN</constant></entry>
+            <entry>menu</entry>
+          </row>
+          <row id="v4l2-test-pattern">
+            <entry spanname="descr"> Some capture/display/sensor devices have
+            the capability to generate test pattern images. These hardware
+            specific test patterns can be used to test if a device is working
+            properly.</entry>
+          </row>
           <row><entry></entry></row>
         </tbody>
       </tgroup>
diff --git a/Documentation/DocBook/media/v4l/io.xml b/Documentation/DocBook/media/v4l/io.xml
index 97f785add841..b5d1cbdc558b 100644
--- a/Documentation/DocBook/media/v4l/io.xml
+++ b/Documentation/DocBook/media/v4l/io.xml
@@ -677,8 +677,10 @@ memory, set by the application. See <xref linkend="userp" /> for details.
             <entry><structfield>length</structfield></entry>
             <entry></entry>
             <entry>Size of the buffer (not the payload) in bytes for the
-            single-planar API. For the multi-planar API should contain the
-            number of elements in the <structfield>planes</structfield> array.
+            single-planar API. For the multi-planar API the application sets
+            this to the number of elements in the <structfield>planes</structfield>
+            array. The driver will fill in the actual number of valid elements in
+            that array.
             </entry>
           </row>
           <row>
diff --git a/Documentation/DocBook/media/v4l/pixfmt-nv12m.xml b/Documentation/DocBook/media/v4l/pixfmt-nv12m.xml
index 5274c24d11e0..a990b34d911a 100644
--- a/Documentation/DocBook/media/v4l/pixfmt-nv12m.xml
+++ b/Documentation/DocBook/media/v4l/pixfmt-nv12m.xml
@@ -1,11 +1,13 @@
-    <refentry id="V4L2-PIX-FMT-NV12M">
+    <refentry>
       <refmeta>
-        <refentrytitle>V4L2_PIX_FMT_NV12M ('NM12')</refentrytitle>
+        <refentrytitle>V4L2_PIX_FMT_NV12M ('NM12'), V4L2_PIX_FMT_NV21M ('NM21'), V4L2_PIX_FMT_NV12MT_16X16</refentrytitle>
         &manvol;
       </refmeta>
       <refnamediv>
-        <refname> <constant>V4L2_PIX_FMT_NV12M</constant></refname>
-        <refpurpose>Variation of <constant>V4L2_PIX_FMT_NV12</constant> with planes
+        <refname id="V4L2-PIX-FMT-NV12M"><constant>V4L2_PIX_FMT_NV12M</constant></refname>
+        <refname id="V4L2-PIX-FMT-NV21M"><constant>V4L2_PIX_FMT_NV21M</constant></refname>
+        <refname id="V4L2-PIX-FMT-NV12MT_16X16"><constant>V4L2_PIX_FMT_NV12MT_16X16</constant></refname>
+        <refpurpose>Variation of <constant>V4L2_PIX_FMT_NV12</constant> and <constant>V4L2_PIX_FMT_NV21</constant> with planes
           non contiguous in memory. </refpurpose>
       </refnamediv>
       <refsect1>
@@ -22,7 +24,12 @@ The CbCr plane is the same width, in bytes, as the Y plane (and of the image),
 but is half as tall in pixels. Each CbCr pair belongs to four pixels. For example,
 Cb<subscript>0</subscript>/Cr<subscript>0</subscript> belongs to
 Y'<subscript>00</subscript>, Y'<subscript>01</subscript>,
-Y'<subscript>10</subscript>, Y'<subscript>11</subscript>. </para>
+Y'<subscript>10</subscript>, Y'<subscript>11</subscript>.
+<constant>V4L2_PIX_FMT_NV12MT_16X16</constant> is the tiled version of
+<constant>V4L2_PIX_FMT_NV12M</constant> with 16x16 macroblock tiles. Here pixels
+are arranged in 16x16 2D tiles and tiles are arranged in linear order in memory.
+<constant>V4L2_PIX_FMT_NV21M</constant> is the same as <constant>V4L2_PIX_FMT_NV12M</constant>
+except the Cb and Cr bytes are swapped, the CrCb plane starts with a Cr byte.</para>
 
         <para><constant>V4L2_PIX_FMT_NV12M</constant> is intended to be
 used only in drivers and applications that support the multi-planar API,
diff --git a/Documentation/DocBook/media/v4l/pixfmt.xml b/Documentation/DocBook/media/v4l/pixfmt.xml
index 1ddbfabe3195..bf94f417592c 100644
--- a/Documentation/DocBook/media/v4l/pixfmt.xml
+++ b/Documentation/DocBook/media/v4l/pixfmt.xml
@@ -758,6 +758,11 @@ extended control <constant>V4L2_CID_MPEG_STREAM_TYPE</constant>, see
                 <entry>'AVC1'</entry>
                 <entry>H264 video elementary stream without start codes.</entry>
           </row>
+          <row id="V4L2-PIX-FMT-H264-MVC">
+                <entry><constant>V4L2_PIX_FMT_H264_MVC</constant></entry>
+                <entry>'MVC'</entry>
+                <entry>H264 MVC video elementary stream.</entry>
+          </row>
           <row id="V4L2-PIX-FMT-H263">
                 <entry><constant>V4L2_PIX_FMT_H263</constant></entry>
                 <entry>'H263'</entry>
@@ -793,6 +798,11 @@ extended control <constant>V4L2_CID_MPEG_STREAM_TYPE</constant>, see
                 <entry>'VC1L'</entry>
                 <entry>VC1, SMPTE 421M Annex L compliant stream.</entry>
           </row>
+          <row id="V4L2-PIX-FMT-VP8">
+                <entry><constant>V4L2_PIX_FMT_VP8</constant></entry>
+                <entry>'VP8'</entry>
+                <entry>VP8 video elementary stream.</entry>
+          </row>
         </tbody>
       </tgroup>
     </table>
@@ -996,6 +1006,34 @@ the other bits are set to 0.</entry>
             <entry>Old 6-bit greyscale format. Only the most significant 6 bits of each byte are used,
 the other bits are set to 0.</entry>
           </row>
+          <row id="V4L2-PIX-FMT-S5C-UYVY-JPG">
+            <entry><constant>V4L2_PIX_FMT_S5C_UYVY_JPG</constant></entry>
+            <entry>'S5CI'</entry>
+            <entry>Two-planar format used by Samsung S5C73MX cameras. The
+first plane contains interleaved JPEG and UYVY image data, followed by meta data
+in form of an array of offsets to the UYVY data blocks. The actual pointer array
+follows immediately the interleaved JPEG/UYVY data, the number of entries in
+this array equals the height of the UYVY image. Each entry is a 4-byte unsigned
+integer in big endian order and it's an offset to a single pixel line of the
+UYVY image. The first plane can start either with JPEG or UYVY data chunk. The
+size of a single UYVY block equals the UYVY image's width multiplied by 2. The
+size of a JPEG chunk depends on the image and can vary with each line.
+<para>The second plane, at an offset of 4084 bytes, contains a 4-byte offset to
+the pointer array in the first plane. This offset is followed by a 4-byte value
+indicating size of the pointer array. All numbers in the second plane are also
+in big endian order. Remaining data in the second plane is undefined. The
+information in the second plane allows to easily find location of the pointer
+array, which can be different for each frame. The size of the pointer array is
+constant for given UYVY image height.</para>
+<para>In order to extract UYVY and JPEG frames an application can initially set
+a data pointer to the start of first plane and then add an offset from the first
+entry of the pointers table. Such a pointer indicates start of an UYVY image
+pixel line. Whole UYVY line can be copied to a separate buffer. These steps
+should be repeated for each line, i.e. the number of entries in the pointer
+array. Anything what's in between the UYVY lines is JPEG data and should be
+concatenated to form the JPEG stream. </para>
+</entry>
+          </row>
         </tbody>
       </tgroup>
     </table>
diff --git a/Documentation/DocBook/media/v4l/subdev-formats.xml b/Documentation/DocBook/media/v4l/subdev-formats.xml
index 49c532ebbbbe..a0a936455fae 100644
--- a/Documentation/DocBook/media/v4l/subdev-formats.xml
+++ b/Documentation/DocBook/media/v4l/subdev-formats.xml
@@ -2565,5 +2565,49 @@
         </tgroup>
       </table>
     </section>
+
+    <section id="v4l2-mbus-vendor-spec-fmts">
+      <title>Vendor and Device Specific Formats</title>
+
+      <note>
+        <title>Experimental</title>
+        <para>This is an <link linkend="experimental">experimental</link>
+interface and may change in the future.</para>
+      </note>
+
+      <para>This section lists complex data formats that are either vendor or
+        device specific.
+      </para>
+
+      <para>The following table lists the existing vendor and device specific
+        formats.</para>
+
+      <table pgwide="0" frame="none" id="v4l2-mbus-pixelcode-vendor-specific">
+        <title>Vendor and device specific formats</title>
+        <tgroup cols="3">
+          <colspec colname="id" align="left" />
+          <colspec colname="code" align="left"/>
+          <colspec colname="remarks" align="left"/>
+          <thead>
+            <row>
+              <entry>Identifier</entry>
+              <entry>Code</entry>
+              <entry>Comments</entry>
+            </row>
+          </thead>
+          <tbody valign="top">
+            <row id="V4L2-MBUS-FMT-S5C-UYVY-JPEG-1X8">
+              <entry>V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8</entry>
+              <entry>0x5001</entry>
+              <entry>
+                Interleaved raw UYVY and JPEG image format with embedded
+                meta-data used by Samsung S3C73MX camera sensors.
+              </entry>
+            </row>
+          </tbody>
+        </tgroup>
+      </table>
+    </section>
+
   </section>
 </section>
diff --git a/Documentation/DocBook/media/v4l/vidioc-qbuf.xml b/Documentation/DocBook/media/v4l/vidioc-qbuf.xml
index 6a821a65a5ae..2d37abefce13 100644
--- a/Documentation/DocBook/media/v4l/vidioc-qbuf.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-qbuf.xml
@@ -121,8 +121,7 @@ remaining fields or returns an error code. The driver may also set
 field. It indicates a non-critical (recoverable) streaming error. In such case
 the application may continue as normal, but should be aware that data in the
 dequeued buffer might be corrupted. When using the multi-planar API, the
-planes array does not have to be passed; the <structfield>m.planes</structfield>
-member must be set to NULL in that case.</para>
+planes array must be passed in as well.</para>
 
     <para>By default <constant>VIDIOC_DQBUF</constant> blocks when no
 buffer is in the outgoing queue. When the
diff --git a/Documentation/DocBook/media/v4l/vidioc-querybuf.xml b/Documentation/DocBook/media/v4l/vidioc-querybuf.xml
index 6e414d7b6df7..a597155c052d 100644
--- a/Documentation/DocBook/media/v4l/vidioc-querybuf.xml
+++ b/Documentation/DocBook/media/v4l/vidioc-querybuf.xml
@@ -48,8 +48,8 @@
   <refsect1>
     <title>Description</title>
 
-    <para>This ioctl is part of the <link linkend="mmap">memory
-mapping</link> I/O method. It can be used to query the status of a
+    <para>This ioctl is part of the <link linkend="mmap">streaming
+</link> I/O method. It can be used to query the status of a
 buffer at any time after buffers have been allocated with the
 &VIDIOC-REQBUFS; ioctl.</para>
 
@@ -71,6 +71,7 @@ the structure.</para>
 
     <para>In the <structfield>flags</structfield> field the
 <constant>V4L2_BUF_FLAG_MAPPED</constant>,
+<constant>V4L2_BUF_FLAG_PREPARED</constant>,
 <constant>V4L2_BUF_FLAG_QUEUED</constant> and
 <constant>V4L2_BUF_FLAG_DONE</constant> flags will be valid. The
 <structfield>memory</structfield> field will be set to the current
@@ -79,8 +80,10 @@ contains the offset of the buffer from the start of the device memory,
 the <structfield>length</structfield> field its size. For the multi-planar API,
 fields <structfield>m.mem_offset</structfield> and
 <structfield>length</structfield> in the <structfield>m.planes</structfield>
-array elements will be used instead. The driver may or may not set the remaining
-fields and flags, they are meaningless in this context.</para>
+array elements will be used instead and the <structfield>length</structfield>
+field of &v4l2-buffer; is set to the number of filled-in array elements.
+The driver may or may not set the remaining fields and flags, they are
+meaningless in this context.</para>
 
     <para>The <structname>v4l2_buffer</structname> structure is
     specified in <xref linkend="buffer" />.</para>
diff --git a/Documentation/video4linux/v4l2-controls.txt b/Documentation/video4linux/v4l2-controls.txt
index 54270df99d5c..cfe52c798d74 100644
--- a/Documentation/video4linux/v4l2-controls.txt
+++ b/Documentation/video4linux/v4l2-controls.txt
@@ -136,11 +136,25 @@ Or alternatively for integer menu controls, by calling v4l2_ctrl_new_int_menu:
                         const struct v4l2_ctrl_ops *ops,
                         u32 id, s32 max, s32 def, const s64 *qmenu_int);
 
+Standard menu controls with a driver specific menu are added by calling
+v4l2_ctrl_new_std_menu_items:
+
+        struct v4l2_ctrl *v4l2_ctrl_new_std_menu_items(
+                struct v4l2_ctrl_handler *hdl,
+                const struct v4l2_ctrl_ops *ops, u32 id, s32 max,
+                s32 skip_mask, s32 def, const char * const *qmenu);
+
 These functions are typically called right after the v4l2_ctrl_handler_init:
 
         static const s64 exp_bias_qmenu[] = {
                -2, -1, 0, 1, 2
         };
+        static const char * const test_pattern[] = {
+                "Disabled",
+                "Vertical Bars",
+                "Solid Black",
+                "Solid White",
+        };
 
         v4l2_ctrl_handler_init(&foo->ctrl_handler, nr_of_controls);
         v4l2_ctrl_new_std(&foo->ctrl_handler, &foo_ctrl_ops,
@@ -156,6 +170,9 @@ These functions are typically called right after the v4l2_ctrl_handler_init:
                         ARRAY_SIZE(exp_bias_qmenu) - 1,
                         ARRAY_SIZE(exp_bias_qmenu) / 2 - 1,
                         exp_bias_qmenu);
+        v4l2_ctrl_new_std_menu_items(&foo->ctrl_handler, &foo_ctrl_ops,
+                        V4L2_CID_TEST_PATTERN, ARRAY_SIZE(test_pattern) - 1, 0,
+                        0, test_pattern);
         ...
         if (foo->ctrl_handler.error) {
                 int err = foo->ctrl_handler.error;
@@ -185,6 +202,13 @@ v4l2_ctrl_new_std_menu in that it doesn't have the mask argument and takes
 as the last argument an array of signed 64-bit integers that form an exact
 menu item list.
 
+The v4l2_ctrl_new_std_menu_items function is very similar to
+v4l2_ctrl_new_std_menu but takes an extra parameter qmenu, which is the driver
+specific menu for an otherwise standard menu control. A good example for this
+control is the test pattern control for capture/display/sensors devices that
+have the capability to generate test patterns. These test patterns are hardware
+specific, so the contents of the menu will vary from device to device.
+
 Note that if something fails, the function will return NULL or an error and
 set ctrl_handler->error to the error code. If ctrl_handler->error was already
 set, then it will just return and do nothing. This is also true for
diff --git a/arch/arm/mach-davinci/Kconfig b/arch/arm/mach-davinci/Kconfig
index ab99c3c3b752..026b4b277ae5 100644
--- a/arch/arm/mach-davinci/Kconfig
+++ b/arch/arm/mach-davinci/Kconfig
@@ -186,6 +186,13 @@ config DA850_UI_RMII
           NOTE: Please take care while choosing this option, MII PHY will
           not be functional if RMII mode is selected.
 
+config DA850_UI_SD_VIDEO_PORT
+        bool "Video Port Interface"
+        help
+          Say Y if you want to use Video Port Interface (VPIF) on the
+          DA850/OMAP-L138 EVM. The Video decoders/encoders are found on the
+          UI daughter card that is supplied with the EVM.
+
 endchoice
 
 config DA850_WL12XX
diff --git a/arch/arm/mach-davinci/board-da850-evm.c b/arch/arm/mach-davinci/board-da850-evm.c
index 1295e616ceee..32ee3f895967 100644
--- a/arch/arm/mach-davinci/board-da850-evm.c
+++ b/arch/arm/mach-davinci/board-da850-evm.c
@@ -45,6 +45,9 @@
 #include <linux/platform_data/mtd-davinci-aemif.h>
 #include <linux/platform_data/spi-davinci.h>
 
+#include <media/tvp514x.h>
+#include <media/adv7343.h>
+
 #define DA850_EVM_PHY_ID                "davinci_mdio-0:00"
 #define DA850_LCD_PWR_PIN               GPIO_TO_PIN(2, 8)
 #define DA850_LCD_BL_PIN                GPIO_TO_PIN(2, 15)
@@ -452,6 +455,15 @@ static void da850_evm_ui_keys_init(unsigned gpio)
         }
 }
 
+#ifdef CONFIG_DA850_UI_SD_VIDEO_PORT
+static inline void da850_evm_setup_video_port(int video_sel)
+{
+        gpio_set_value_cansleep(video_sel, 0);
+}
+#else
+static inline void da850_evm_setup_video_port(int video_sel) { }
+#endif
+
 static int da850_evm_ui_expander_setup(struct i2c_client *client, unsigned gpio,
                                                 unsigned ngpio, void *c)
 {
@@ -497,6 +509,8 @@ static int da850_evm_ui_expander_setup(struct i2c_client *client, unsigned gpio,
 
         da850_evm_setup_emac_rmii(sel_a);
 
+        da850_evm_setup_video_port(sel_c);
+
         return 0;
 
 exp_setup_keys_fail:
@@ -1149,6 +1163,169 @@ static __init int da850_evm_init_cpufreq(void)
 static __init int da850_evm_init_cpufreq(void) { return 0; }
 #endif
 
+#if defined(CONFIG_DA850_UI_SD_VIDEO_PORT)
+
+#define TVP5147_CH0             "tvp514x-0"
+#define TVP5147_CH1             "tvp514x-1"
+
+/* VPIF capture configuration */
+static struct tvp514x_platform_data tvp5146_pdata = {
+                .clk_polarity = 0,
+                .hs_polarity  = 1,
+                .vs_polarity  = 1,
+};
+
+#define TVP514X_STD_ALL (V4L2_STD_NTSC | V4L2_STD_PAL)
+
+static const struct vpif_input da850_ch0_inputs[] = {
+        {
+                .input = {
+                        .index = 0,
+                        .name  = "Composite",
+                        .type  = V4L2_INPUT_TYPE_CAMERA,
+                        .capabilities = V4L2_IN_CAP_STD,
+                        .std   = TVP514X_STD_ALL,
+                },
+                .input_route = INPUT_CVBS_VI2B,
+                .output_route = OUTPUT_10BIT_422_EMBEDDED_SYNC,
+                .subdev_name = TVP5147_CH0,
+        },
+};
+
+static const struct vpif_input da850_ch1_inputs[] = {
+        {
+                .input = {
+                        .index = 0,
+                        .name  = "S-Video",
+                        .type  = V4L2_INPUT_TYPE_CAMERA,
+                        .capabilities = V4L2_IN_CAP_STD,
+                        .std   = TVP514X_STD_ALL,
+                },
+                .input_route = INPUT_SVIDEO_VI2C_VI1C,
+                .output_route = OUTPUT_10BIT_422_EMBEDDED_SYNC,
+                .subdev_name = TVP5147_CH1,
+        },
+};
+
+static struct vpif_subdev_info da850_vpif_capture_sdev_info[] = {
+        {
+                .name = TVP5147_CH0,
+                .board_info = {
+                        I2C_BOARD_INFO("tvp5146", 0x5d),
+                        .platform_data = &tvp5146_pdata,
+                },
+        },
+        {
+                .name = TVP5147_CH1,
+                .board_info = {
+                        I2C_BOARD_INFO("tvp5146", 0x5c),
+                        .platform_data = &tvp5146_pdata,
+                },
+        },
+};
+
+static struct vpif_capture_config da850_vpif_capture_config = {
+        .subdev_info = da850_vpif_capture_sdev_info,
+        .subdev_count = ARRAY_SIZE(da850_vpif_capture_sdev_info),
+        .chan_config[0] = {
+                .inputs = da850_ch0_inputs,
+                .input_count = ARRAY_SIZE(da850_ch0_inputs),
+                .vpif_if = {
+                        .if_type = VPIF_IF_BT656,
+                        .hd_pol  = 1,
+                        .vd_pol  = 1,
+                        .fid_pol = 0,
+                },
+        },
+        .chan_config[1] = {
+                .inputs = da850_ch1_inputs,
+                .input_count = ARRAY_SIZE(da850_ch1_inputs),
+                .vpif_if = {
+                        .if_type = VPIF_IF_BT656,
+                        .hd_pol  = 1,
+                        .vd_pol  = 1,
+                        .fid_pol = 0,
+                },
+        },
+        .card_name = "DA850/OMAP-L138 Video Capture",
+};
+
+/* VPIF display configuration */
+static struct vpif_subdev_info da850_vpif_subdev[] = {
+        {
+                .name = "adv7343",
+                .board_info = {
+                        I2C_BOARD_INFO("adv7343", 0x2a),
+                },
+        },
+};
+
+static const struct vpif_output da850_ch0_outputs[] = {
+        {
+                .output = {
+                        .index = 0,
+                        .name = "Composite",
+                        .type = V4L2_OUTPUT_TYPE_ANALOG,
+                        .capabilities = V4L2_OUT_CAP_STD,
+                        .std = V4L2_STD_ALL,
+                },
+                .subdev_name = "adv7343",
+                .output_route = ADV7343_COMPOSITE_ID,
+        },
+        {
+                .output = {
+                        .index = 1,
+                        .name = "S-Video",
+                        .type = V4L2_OUTPUT_TYPE_ANALOG,
+                        .capabilities = V4L2_OUT_CAP_STD,
+                        .std = V4L2_STD_ALL,
+                },
+                .subdev_name = "adv7343",
+                .output_route = ADV7343_SVIDEO_ID,
+        },
+};
+
+static struct vpif_display_config da850_vpif_display_config = {
+        .subdevinfo   = da850_vpif_subdev,
+        .subdev_count = ARRAY_SIZE(da850_vpif_subdev),
+        .chan_config[0] = {
+                .outputs = da850_ch0_outputs,
+                .output_count = ARRAY_SIZE(da850_ch0_outputs),
+        },
+        .card_name    = "DA850/OMAP-L138 Video Display",
+};
+
+static __init void da850_vpif_init(void)
+{
+        int ret;
+
+        ret = da850_register_vpif();
+        if (ret)
+                pr_warn("da850_evm_init: VPIF setup failed: %d\n", ret);
+
+        ret = davinci_cfg_reg_list(da850_vpif_capture_pins);
+        if (ret)
+                pr_warn("da850_evm_init: VPIF capture mux setup failed: %d\n",
+                        ret);
+
+        ret = da850_register_vpif_capture(&da850_vpif_capture_config);
+        if (ret)
+                pr_warn("da850_evm_init: VPIF capture setup failed: %d\n", ret);
+
+        ret = davinci_cfg_reg_list(da850_vpif_display_pins);
+        if (ret)
+                pr_warn("da850_evm_init: VPIF display mux setup failed: %d\n",
+                        ret);
+
+        ret = da850_register_vpif_display(&da850_vpif_display_config);
+        if (ret)
+                pr_warn("da850_evm_init: VPIF display setup failed: %d\n", ret);
+}
+
+#else
+static __init void da850_vpif_init(void) {}
+#endif
+
 #ifdef CONFIG_DA850_WL12XX
 
 static void wl12xx_set_power(int index, bool power_on)
@@ -1375,6 +1552,8 @@ static __init void da850_evm_init(void)
                 pr_warning("da850_evm_init: suspend registration failed: %d\n",
                                 ret);
 
+        da850_vpif_init();
+
         ret = da8xx_register_spi(1, da850evm_spi_info,
                                  ARRAY_SIZE(da850evm_spi_info));
         if (ret)
diff --git a/arch/arm/mach-davinci/board-dm644x-evm.c b/arch/arm/mach-davinci/board-dm644x-evm.c
index ca72fc4b8cca..f22572cee49d 100644
--- a/arch/arm/mach-davinci/board-dm644x-evm.c
+++ b/arch/arm/mach-davinci/board-dm644x-evm.c
@@ -23,6 +23,7 @@
 #include <linux/phy.h>
 #include <linux/clk.h>
 #include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
 #include <linux/export.h>
 
 #include <media/tvp514x.h>
@@ -620,7 +621,7 @@ static struct vpbe_enc_mode_info dm644xevm_enc_std_timing[] = {
         {
                 .name           = "ntsc",
                 .timings_type   = VPBE_ENC_STD,
-                .timings        = {V4L2_STD_525_60},
+                .std_id         = V4L2_STD_525_60,
                 .interlaced     = 1,
                 .xres           = 720,
                 .yres           = 480,
@@ -632,7 +633,7 @@ static struct vpbe_enc_mode_info dm644xevm_enc_std_timing[] = {
         {
                 .name           = "pal",
                 .timings_type   = VPBE_ENC_STD,
-                .timings        = {V4L2_STD_625_50},
+                .std_id         = V4L2_STD_625_50,
                 .interlaced     = 1,
                 .xres           = 720,
                 .yres           = 576,
@@ -647,8 +648,8 @@ static struct vpbe_enc_mode_info dm644xevm_enc_std_timing[] = {
 static struct vpbe_enc_mode_info dm644xevm_enc_preset_timing[] = {
         {
                 .name           = "480p59_94",
-                .timings_type   = VPBE_ENC_DV_PRESET,
-                .timings        = {V4L2_DV_480P59_94},
+                .timings_type   = VPBE_ENC_CUSTOM_TIMINGS,
+                .dv_timings     = V4L2_DV_BT_CEA_720X480P59_94,
                 .interlaced     = 0,
                 .xres           = 720,
                 .yres           = 480,
@@ -659,8 +660,8 @@ static struct vpbe_enc_mode_info dm644xevm_enc_preset_timing[] = {
         },
         {
                 .name           = "576p50",
-                .timings_type   = VPBE_ENC_DV_PRESET,
-                .timings        = {V4L2_DV_576P50},
+                .timings_type   = VPBE_ENC_CUSTOM_TIMINGS,
+                .dv_timings     = V4L2_DV_BT_CEA_720X576P50,
                 .interlaced     = 0,
                 .xres           = 720,
                 .yres           = 576,
@@ -698,7 +699,7 @@ static struct vpbe_output dm644xevm_vpbe_outputs[] = {
                         .index          = 1,
                         .name           = "Component",
                         .type           = V4L2_OUTPUT_TYPE_ANALOG,
-                        .capabilities   = V4L2_OUT_CAP_PRESETS,
+                        .capabilities   = V4L2_OUT_CAP_DV_TIMINGS,
                 },
                 .subdev_name    = VPBE_VENC_SUBDEV_NAME,
                 .default_mode   = "480p59_94",
diff --git a/arch/arm/mach-davinci/board-dm646x-evm.c b/arch/arm/mach-davinci/board-dm646x-evm.c
index 9944367b4931..1dbf85beed1b 100644
--- a/arch/arm/mach-davinci/board-dm646x-evm.c
+++ b/arch/arm/mach-davinci/board-dm646x-evm.c
@@ -26,6 +26,7 @@
 #include <linux/i2c/pcf857x.h>
 
 #include <media/tvp514x.h>
+#include <media/adv7343.h>
 
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/nand.h>
@@ -496,18 +497,49 @@ static struct vpif_subdev_info dm646x_vpif_subdev[] = {
         },
 };
 
-static const char *output[] = {
-        "Composite",
-        "Component",
-        "S-Video",
+static const struct vpif_output dm6467_ch0_outputs[] = {
+        {
+                .output = {
+                        .index = 0,
+                        .name = "Composite",
+                        .type = V4L2_OUTPUT_TYPE_ANALOG,
+                        .capabilities = V4L2_OUT_CAP_STD,
+                        .std = V4L2_STD_ALL,
+                },
+                .subdev_name = "adv7343",
+                .output_route = ADV7343_COMPOSITE_ID,
+        },
+        {
+                .output = {
+                        .index = 1,
+                        .name = "Component",
+                        .type = V4L2_OUTPUT_TYPE_ANALOG,
+                        .capabilities = V4L2_OUT_CAP_CUSTOM_TIMINGS,
+                },
+                .subdev_name = "adv7343",
+                .output_route = ADV7343_COMPONENT_ID,
+        },
+        {
+                .output = {
+                        .index = 2,
+                        .name = "S-Video",
+                        .type = V4L2_OUTPUT_TYPE_ANALOG,
+                        .capabilities = V4L2_OUT_CAP_STD,
+                        .std = V4L2_STD_ALL,
+                },
+                .subdev_name = "adv7343",
+                .output_route = ADV7343_SVIDEO_ID,
+        },
 };
 
 static struct vpif_display_config dm646x_vpif_display_config = {
         .set_clock      = set_vpif_clock,
         .subdevinfo     = dm646x_vpif_subdev,
         .subdev_count   = ARRAY_SIZE(dm646x_vpif_subdev),
-        .output         = output,
-        .output_count   = ARRAY_SIZE(output),
+        .chan_config[0] = {
+                .outputs = dm6467_ch0_outputs,
+                .output_count = ARRAY_SIZE(dm6467_ch0_outputs),
+        },
         .card_name      = "DM646x EVM",
 };
 
@@ -601,15 +633,6 @@ static struct vpif_subdev_info vpif_capture_sdev_info[] = {
                         I2C_BOARD_INFO("tvp5146", 0x5d),
                         .platform_data = &tvp5146_pdata,
                 },
-                .input = INPUT_CVBS_VI2B,
-                .output = OUTPUT_10BIT_422_EMBEDDED_SYNC,
-                .can_route = 1,
-                .vpif_if = {
-                        .if_type = VPIF_IF_BT656,
-                        .hd_pol = 1,
-                        .vd_pol = 1,
-                        .fid_pol = 0,
-                },
         },
         {
                 .name   = TVP5147_CH1,
@@ -617,15 +640,6 @@ static struct vpif_subdev_info vpif_capture_sdev_info[] = {
                         I2C_BOARD_INFO("tvp5146", 0x5c),
                         .platform_data = &tvp5146_pdata,
                 },
-                .input = INPUT_SVIDEO_VI2C_VI1C,
-                .output = OUTPUT_10BIT_422_EMBEDDED_SYNC,
-                .can_route = 1,
-                .vpif_if = {
-                        .if_type = VPIF_IF_BT656,
-                        .hd_pol = 1,
-                        .vd_pol = 1,
-                        .fid_pol = 0,
-                },
         },
 };
 
@@ -635,9 +649,12 @@ static const struct vpif_input dm6467_ch0_inputs[] = {
                         .index = 0,
                         .name = "Composite",
                         .type = V4L2_INPUT_TYPE_CAMERA,
+                        .capabilities = V4L2_IN_CAP_STD,
                         .std = TVP514X_STD_ALL,
                 },
                 .subdev_name = TVP5147_CH0,
+                .input_route = INPUT_CVBS_VI2B,
+                .output_route = OUTPUT_10BIT_422_EMBEDDED_SYNC,
         },
 };
 
@@ -647,9 +664,12 @@ static const struct vpif_input dm6467_ch1_inputs[] = {
                         .index = 0,
                         .name = "S-Video",
                         .type = V4L2_INPUT_TYPE_CAMERA,
+                        .capabilities = V4L2_IN_CAP_STD,
                         .std = TVP514X_STD_ALL,
                 },
                 .subdev_name = TVP5147_CH1,
+                .input_route = INPUT_SVIDEO_VI2C_VI1C,
+                .output_route = OUTPUT_10BIT_422_EMBEDDED_SYNC,
         },
 };
 
@@ -661,10 +681,22 @@ static struct vpif_capture_config dm646x_vpif_capture_cfg = {
         .chan_config[0] = {
                 .inputs = dm6467_ch0_inputs,
                 .input_count = ARRAY_SIZE(dm6467_ch0_inputs),
+                .vpif_if = {
+                        .if_type = VPIF_IF_BT656,
+                        .hd_pol = 1,
+                        .vd_pol = 1,
+                        .fid_pol = 0,
+                },
         },
         .chan_config[1] = {
                 .inputs = dm6467_ch1_inputs,
                 .input_count = ARRAY_SIZE(dm6467_ch1_inputs),
+                .vpif_if = {
+                        .if_type = VPIF_IF_BT656,
+                        .hd_pol = 1,
+                        .vd_pol = 1,
+                        .fid_pol = 0,
+                },
         },
 };
 
diff --git a/arch/arm/mach-davinci/da850.c b/arch/arm/mach-davinci/da850.c
index e517e1036b09..b90c172d5541 100644
--- a/arch/arm/mach-davinci/da850.c
+++ b/arch/arm/mach-davinci/da850.c
@@ -347,6 +347,13 @@ static struct clk spi1_clk = {
         .flags          = DA850_CLK_ASYNC3,
 };
 
+static struct clk vpif_clk = {
+        .name           = "vpif",
+        .parent         = &pll0_sysclk2,
+        .lpsc           = DA850_LPSC1_VPIF,
+        .gpsc           = 1,
+};
+
 static struct clk sata_clk = {
         .name           = "sata",
         .parent         = &pll0_sysclk2,
@@ -397,6 +404,7 @@ static struct clk_lookup da850_clks[] = {
         CLK(NULL,               "usb20",        &usb20_clk),
         CLK("spi_davinci.0",    NULL,           &spi0_clk),
         CLK("spi_davinci.1",    NULL,           &spi1_clk),
+        CLK("vpif",             NULL,           &vpif_clk),
         CLK("ahci",             NULL,           &sata_clk),
         CLK(NULL,               NULL,           NULL),
 };
@@ -573,6 +581,46 @@ static const struct mux_config da850_pins[] = {
         MUX_CFG(DA850, GPIO6_10,        13,     20,     15,     8,      false)
         MUX_CFG(DA850, GPIO6_13,        13,     8,      15,     8,      false)
         MUX_CFG(DA850, RTC_ALARM,       0,      28,     15,     2,      false)
+        /* VPIF Capture */
+        MUX_CFG(DA850, VPIF_DIN0,       15,     4,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN1,       15,     0,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN2,       14,     28,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN3,       14,     24,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN4,       14,     20,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN5,       14,     16,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN6,       14,     12,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN7,       14,     8,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN8,       16,     4,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN9,       16,     0,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN10,      15,     28,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN11,      15,     24,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN12,      15,     20,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN13,      15,     16,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN14,      15,     12,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DIN15,      15,     8,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_CLKIN0,     14,     0,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_CLKIN1,     14,     4,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_CLKIN2,     19,     8,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_CLKIN3,     19,     16,     15,     1,      false)
+        /* VPIF Display */
+        MUX_CFG(DA850, VPIF_DOUT0,      17,     4,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT1,      17,     0,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT2,      16,     28,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT3,      16,     24,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT4,      16,     20,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT5,      16,     16,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT6,      16,     12,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT7,      16,     8,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT8,      18,     4,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT9,      18,     0,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT10,     17,     28,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT11,     17,     24,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT12,     17,     20,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT13,     17,     16,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT14,     17,     12,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_DOUT15,     17,     8,      15,     1,      false)
+        MUX_CFG(DA850, VPIF_CLKO2,      19,     12,     15,     1,      false)
+        MUX_CFG(DA850, VPIF_CLKO3,      19,     20,     15,     1,      false)
 #endif
 };
 
@@ -595,6 +643,26 @@ const short da850_lcdcntl_pins[] __initconst = {
         -1
 };
 
+const short da850_vpif_capture_pins[] __initdata = {
+        DA850_VPIF_DIN0, DA850_VPIF_DIN1, DA850_VPIF_DIN2, DA850_VPIF_DIN3,
+        DA850_VPIF_DIN4, DA850_VPIF_DIN5, DA850_VPIF_DIN6, DA850_VPIF_DIN7,
+        DA850_VPIF_DIN8, DA850_VPIF_DIN9, DA850_VPIF_DIN10, DA850_VPIF_DIN11,
+        DA850_VPIF_DIN12, DA850_VPIF_DIN13, DA850_VPIF_DIN14, DA850_VPIF_DIN15,
+        DA850_VPIF_CLKIN0, DA850_VPIF_CLKIN1, DA850_VPIF_CLKIN2,
+        DA850_VPIF_CLKIN3,
+        -1
+};
+
+const short da850_vpif_display_pins[] __initdata = {
+        DA850_VPIF_DOUT0, DA850_VPIF_DOUT1, DA850_VPIF_DOUT2, DA850_VPIF_DOUT3,
+        DA850_VPIF_DOUT4, DA850_VPIF_DOUT5, DA850_VPIF_DOUT6, DA850_VPIF_DOUT7,
+        DA850_VPIF_DOUT8, DA850_VPIF_DOUT9, DA850_VPIF_DOUT10,
+        DA850_VPIF_DOUT11, DA850_VPIF_DOUT12, DA850_VPIF_DOUT13,
+        DA850_VPIF_DOUT14, DA850_VPIF_DOUT15, DA850_VPIF_CLKO2,
+        DA850_VPIF_CLKO3,
+        -1
+};
+
 /* FIQ are pri 0-1; otherwise 2-7, with 7 lowest priority */
 static u8 da850_default_priorities[DA850_N_CP_INTC_IRQ] = {
         [IRQ_DA8XX_COMMTX]              = 7,
@@ -1064,6 +1132,90 @@ no_ddrpll_mem:
         return ret;
 }
 
+/* VPIF resource, platform data */
+static u64 da850_vpif_dma_mask = DMA_BIT_MASK(32);
+
+static struct resource da850_vpif_resource[] = {
+        {
+                .start = DA8XX_VPIF_BASE,
+                .end   = DA8XX_VPIF_BASE + 0xfff,
+                .flags = IORESOURCE_MEM,
+        }
+};
+
+static struct platform_device da850_vpif_dev = {
+        .name           = "vpif",
+        .id             = -1,
+        .dev            = {
+                .dma_mask               = &da850_vpif_dma_mask,
+                .coherent_dma_mask      = DMA_BIT_MASK(32),
+        },
+        .resource       = da850_vpif_resource,
+        .num_resources  = ARRAY_SIZE(da850_vpif_resource),
+};
+
+static struct resource da850_vpif_display_resource[] = {
+        {
+                .start = IRQ_DA850_VPIFINT,
+                .end   = IRQ_DA850_VPIFINT,
+                .flags = IORESOURCE_IRQ,
+        },
+};
+
+static struct platform_device da850_vpif_display_dev = {
+        .name           = "vpif_display",
+        .id             = -1,
+        .dev            = {
+                .dma_mask               = &da850_vpif_dma_mask,
+                .coherent_dma_mask      = DMA_BIT_MASK(32),
+        },
+        .resource       = da850_vpif_display_resource,
+        .num_resources  = ARRAY_SIZE(da850_vpif_display_resource),
+};
+
+static struct resource da850_vpif_capture_resource[] = {
+        {
+                .start = IRQ_DA850_VPIFINT,
+                .end   = IRQ_DA850_VPIFINT,
+                .flags = IORESOURCE_IRQ,
+        },
+        {
+                .start = IRQ_DA850_VPIFINT,
+                .end   = IRQ_DA850_VPIFINT,
+                .flags = IORESOURCE_IRQ,
+        },
+};
+
+static struct platform_device da850_vpif_capture_dev = {
+        .name           = "vpif_capture",
+        .id             = -1,
+        .dev            = {
+                .dma_mask               = &da850_vpif_dma_mask,
+                .coherent_dma_mask      = DMA_BIT_MASK(32),
+        },
+        .resource       = da850_vpif_capture_resource,
+        .num_resources  = ARRAY_SIZE(da850_vpif_capture_resource),
+};
+
+int __init da850_register_vpif(void)
+{
+        return platform_device_register(&da850_vpif_dev);
+}
+
+int __init da850_register_vpif_display(struct vpif_display_config
+                                                *display_config)
+{
+        da850_vpif_display_dev.dev.platform_data = display_config;
+        return platform_device_register(&da850_vpif_display_dev);
+}
+
+int __init da850_register_vpif_capture(struct vpif_capture_config
+                                                        *capture_config)
+{
+        da850_vpif_capture_dev.dev.platform_data = capture_config;
+        return platform_device_register(&da850_vpif_capture_dev);
+}
+
 static struct davinci_soc_info davinci_soc_info_da850 = {
         .io_desc                = da850_io_desc,
         .io_desc_num            = ARRAY_SIZE(da850_io_desc),
diff --git a/arch/arm/mach-davinci/dm644x.c b/arch/arm/mach-davinci/dm644x.c
index 0755d466221a..cd0c8b1e1ecf 100644
--- a/arch/arm/mach-davinci/dm644x.c
+++ b/arch/arm/mach-davinci/dm644x.c
@@ -701,7 +701,7 @@ static struct resource dm644x_venc_resources[] = {
 #define DM644X_VPSS_DACCLKEN                  BIT(4)
 
 static int dm644x_venc_setup_clock(enum vpbe_enc_timings_type type,
-                                   unsigned int mode)
+                                   unsigned int pclock)
 {
         int ret = 0;
         u32 v = DM644X_VPSS_VENCLKEN;
@@ -711,27 +711,18 @@ static int dm644x_venc_setup_clock(enum vpbe_enc_timings_type type,
                 v |= DM644X_VPSS_DACCLKEN;
                 writel(v, DAVINCI_SYSMOD_VIRT(SYSMOD_VPSS_CLKCTL));
                 break;
-        case VPBE_ENC_DV_PRESET:
-                switch (mode) {
-                case V4L2_DV_480P59_94:
-                case V4L2_DV_576P50:
+        case VPBE_ENC_CUSTOM_TIMINGS:
+                if (pclock <= 27000000) {
                         v |= DM644X_VPSS_MUXSEL_PLL2_MODE |
                              DM644X_VPSS_DACCLKEN;
                         writel(v, DAVINCI_SYSMOD_VIRT(SYSMOD_VPSS_CLKCTL));
-                        break;
-                case V4L2_DV_720P60:
-                case V4L2_DV_1080I60:
-                case V4L2_DV_1080P30:
+                } else {
                         /*
                          * For HD, use external clock source since
                          * HD requires higher clock rate
                          */
                         v |= DM644X_VPSS_MUXSEL_VPBECLK_MODE;
                         writel(v, DAVINCI_SYSMOD_VIRT(SYSMOD_VPSS_CLKCTL));
-                        break;
-                default:
-                        ret = -EINVAL;
-                        break;
                 }
                 break;
         default:
diff --git a/arch/arm/mach-davinci/include/mach/da8xx.h b/arch/arm/mach-davinci/include/mach/da8xx.h
index c9ee723c56f3..aaccdc4528fc 100644
--- a/arch/arm/mach-davinci/include/mach/da8xx.h
+++ b/arch/arm/mach-davinci/include/mach/da8xx.h
@@ -17,6 +17,7 @@
 #include <linux/davinci_emac.h>
 #include <linux/spi/spi.h>
 #include <linux/platform_data/davinci_asp.h>
+#include <linux/videodev2.h>
 
 #include <mach/serial.h>
 #include <mach/edma.h>
@@ -26,6 +27,8 @@
 #include <linux/platform_data/usb-davinci.h>
 #include <linux/platform_data/spi-davinci.h>
 
+#include <media/davinci/vpif_types.h>
+
 extern void __iomem *da8xx_syscfg0_base;
 extern void __iomem *da8xx_syscfg1_base;
 
@@ -63,6 +66,7 @@ extern unsigned int da850_max_speed;
 #define DA8XX_PLL0_BASE         0x01c11000
 #define DA8XX_TIMER64P0_BASE    0x01c20000
 #define DA8XX_TIMER64P1_BASE    0x01c21000
+#define DA8XX_VPIF_BASE         0x01e17000
 #define DA8XX_GPIO_BASE         0x01e26000
 #define DA8XX_PSC1_BASE         0x01e27000
 #define DA8XX_AEMIF_CS2_BASE    0x60000000
@@ -92,6 +96,11 @@ int da8xx_register_cpuidle(void);
 void __iomem * __init da8xx_get_mem_ctlr(void);
 int da850_register_pm(struct platform_device *pdev);
 int __init da850_register_sata(unsigned long refclkpn);
+int __init da850_register_vpif(void);
+int __init da850_register_vpif_display
+                        (struct vpif_display_config *display_config);
+int __init da850_register_vpif_capture
+                        (struct vpif_capture_config *capture_config);
 void da8xx_restart(char mode, const char *cmd);
 
 extern struct platform_device da8xx_serial_device;
@@ -126,6 +135,8 @@ extern const short da830_ecap1_pins[];
 extern const short da830_ecap2_pins[];
 extern const short da830_eqep0_pins[];
 extern const short da830_eqep1_pins[];
+extern const short da850_vpif_capture_pins[];
+extern const short da850_vpif_display_pins[];
 
 extern const short da850_i2c0_pins[];
 extern const short da850_i2c1_pins[];
diff --git a/arch/arm/mach-davinci/include/mach/mux.h b/arch/arm/mach-davinci/include/mach/mux.h
index a7e92fca32e6..9e95b8a1edb6 100644
--- a/arch/arm/mach-davinci/include/mach/mux.h
+++ b/arch/arm/mach-davinci/include/mach/mux.h
@@ -928,6 +928,48 @@ enum davinci_da850_index {
         DA850_GPIO6_10,
         DA850_GPIO6_13,
         DA850_RTC_ALARM,
+
+        /* VPIF Capture */
+        DA850_VPIF_DIN0,
+        DA850_VPIF_DIN1,
+        DA850_VPIF_DIN2,
+        DA850_VPIF_DIN3,
+        DA850_VPIF_DIN4,
+        DA850_VPIF_DIN5,
+        DA850_VPIF_DIN6,
+        DA850_VPIF_DIN7,
+        DA850_VPIF_DIN8,
+        DA850_VPIF_DIN9,
+        DA850_VPIF_DIN10,
+        DA850_VPIF_DIN11,
+        DA850_VPIF_DIN12,
+        DA850_VPIF_DIN13,
+        DA850_VPIF_DIN14,
+        DA850_VPIF_DIN15,
+        DA850_VPIF_CLKIN0,
+        DA850_VPIF_CLKIN1,
+        DA850_VPIF_CLKIN2,
+        DA850_VPIF_CLKIN3,
+
+        /* VPIF Display */
+        DA850_VPIF_DOUT0,
+        DA850_VPIF_DOUT1,
+        DA850_VPIF_DOUT2,
+        DA850_VPIF_DOUT3,
+        DA850_VPIF_DOUT4,
+        DA850_VPIF_DOUT5,
+        DA850_VPIF_DOUT6,
+        DA850_VPIF_DOUT7,
+        DA850_VPIF_DOUT8,
+        DA850_VPIF_DOUT9,
+        DA850_VPIF_DOUT10,
+        DA850_VPIF_DOUT11,
+        DA850_VPIF_DOUT12,
+        DA850_VPIF_DOUT13,
+        DA850_VPIF_DOUT14,
+        DA850_VPIF_DOUT15,
+        DA850_VPIF_CLKO2,
+        DA850_VPIF_CLKO3,
 };
 
 enum davinci_tnetv107x_index {
diff --git a/arch/arm/mach-davinci/include/mach/psc.h b/arch/arm/mach-davinci/include/mach/psc.h
index 405318e35bf6..40a0027838e8 100644
--- a/arch/arm/mach-davinci/include/mach/psc.h
+++ b/arch/arm/mach-davinci/include/mach/psc.h
@@ -166,6 +166,7 @@
 #define DA830_LPSC1_McASP1              8
 #define DA850_LPSC1_SATA                8
 #define DA830_LPSC1_McASP2              9
+#define DA850_LPSC1_VPIF                9
 #define DA8XX_LPSC1_SPI1                10
 #define DA8XX_LPSC1_I2C                 11
 #define DA8XX_LPSC1_UART1               12
diff --git a/arch/arm/mach-exynos/mach-nuri.c b/arch/arm/mach-exynos/mach-nuri.c
index 85fc2feb0af0..c05d7aa84031 100644
--- a/arch/arm/mach-exynos/mach-nuri.c
+++ b/arch/arm/mach-exynos/mach-nuri.c
@@ -378,10 +378,10 @@ static struct regulator_consumer_supply __initdata max8997_ldo1_[] = {
 };
 static struct regulator_consumer_supply __initdata max8997_ldo3_[] = {
         REGULATOR_SUPPLY("vusb_d", "s3c-hsotg"), /* USB */
-        REGULATOR_SUPPLY("vdd11", "s5p-mipi-csis.0"), /* MIPI */
+        REGULATOR_SUPPLY("vddcore", "s5p-mipi-csis.0"), /* MIPI */
 };
 static struct regulator_consumer_supply __initdata max8997_ldo4_[] = {
-        REGULATOR_SUPPLY("vdd18", "s5p-mipi-csis.0"), /* MIPI */
+        REGULATOR_SUPPLY("vddio", "s5p-mipi-csis.0"), /* MIPI */
 };
 static struct regulator_consumer_supply __initdata max8997_ldo5_[] = {
         REGULATOR_SUPPLY("vhsic", "modemctl"), /* MODEM */
@@ -1180,9 +1180,7 @@ static struct platform_device cam_8m_12v_fixed_rdev = {
 static struct s5p_platform_mipi_csis mipi_csis_platdata = {
         .clk_rate       = 166000000UL,
         .lanes          = 2,
-        .alignment      = 32,
         .hs_settle      = 12,
-        .phy_enable     = s5p_csis_phy_enable,
 };
 
 #define GPIO_CAM_MEGA_RST       EXYNOS4_GPY3(7) /* ISP_RESET */
@@ -1226,7 +1224,6 @@ static struct s5p_fimc_isp_info nuri_camera_sensors[] = {
                 .bus_type       = FIMC_MIPI_CSI2,
                 .board_info     = &m5mols_board_info,
                 .clk_frequency  = 24000000UL,
-                .csi_data_align = 32,
         },
 };
 
diff --git a/arch/arm/mach-exynos/mach-origen.c b/arch/arm/mach-exynos/mach-origen.c
index c11a62e23dcc..9adf491674ea 100644
--- a/arch/arm/mach-exynos/mach-origen.c
+++ b/arch/arm/mach-exynos/mach-origen.c
@@ -97,12 +97,12 @@ static struct s3c2410_uartcfg origen_uartcfgs[] __initdata = {
 };
 
 static struct regulator_consumer_supply __initdata ldo3_consumer[] = {
-        REGULATOR_SUPPLY("vdd11", "s5p-mipi-csis.0"), /* MIPI */
+        REGULATOR_SUPPLY("vddcore", "s5p-mipi-csis.0"), /* MIPI */
         REGULATOR_SUPPLY("vdd", "exynos4-hdmi"), /* HDMI */
         REGULATOR_SUPPLY("vdd_pll", "exynos4-hdmi"), /* HDMI */
 };
 static struct regulator_consumer_supply __initdata ldo6_consumer[] = {
-        REGULATOR_SUPPLY("vdd18", "s5p-mipi-csis.0"), /* MIPI */
+        REGULATOR_SUPPLY("vddio", "s5p-mipi-csis.0"), /* MIPI */
 };
 static struct regulator_consumer_supply __initdata ldo7_consumer[] = {
         REGULATOR_SUPPLY("avdd", "alc5625"), /* Realtek ALC5625 */
diff --git a/arch/arm/mach-exynos/mach-universal_c210.c b/arch/arm/mach-exynos/mach-universal_c210.c
index 08b81fd99505..ebc9dd339a38 100644
--- a/arch/arm/mach-exynos/mach-universal_c210.c
+++ b/arch/arm/mach-exynos/mach-universal_c210.c
@@ -209,7 +209,7 @@ static struct regulator_consumer_supply lp3974_ldo3_consumer[] = {
         REGULATOR_SUPPLY("vusb_a", "s3c-hsotg"),
         REGULATOR_SUPPLY("vdd", "exynos4-hdmi"),
         REGULATOR_SUPPLY("vdd_pll", "exynos4-hdmi"),
-        REGULATOR_SUPPLY("vdd11", "s5p-mipi-csis.0"),
+        REGULATOR_SUPPLY("vddcore", "s5p-mipi-csis.0"),
 };
 
 static struct regulator_init_data lp3974_ldo3_data = {
@@ -273,7 +273,7 @@ static struct regulator_init_data lp3974_ldo6_data = {
 };
 
 static struct regulator_consumer_supply lp3974_ldo7_consumer[] = {
-        REGULATOR_SUPPLY("vdd18", "s5p-mipi-csis.0"),
+        REGULATOR_SUPPLY("vddio", "s5p-mipi-csis.0"),
 };
 
 static struct regulator_init_data lp3974_ldo7_data = {
@@ -942,9 +942,7 @@ static struct platform_device cam_s_if_fixed_reg_dev = {
 static struct s5p_platform_mipi_csis mipi_csis_platdata = {
         .clk_rate       = 166000000UL,
         .lanes          = 2,
-        .alignment      = 32,
         .hs_settle      = 12,
-        .phy_enable     = s5p_csis_phy_enable,
 };
 
 #define GPIO_CAM_LEVEL_EN(n)    EXYNOS4_GPE4(n + 3)
@@ -1008,7 +1006,6 @@ static struct s5p_fimc_isp_info universal_camera_sensors[] = {
                 .board_info     = &m5mols_board_info,
                 .i2c_bus_num    = 0,
                 .clk_frequency  = 24000000UL,
-                .csi_data_align = 32,
         },
 };
 
diff --git a/arch/arm/plat-samsung/setup-mipiphy.c b/arch/arm/plat-samsung/setup-mipiphy.c
index 683c466c0e6a..147459327601 100644
--- a/arch/arm/plat-samsung/setup-mipiphy.c
+++ b/arch/arm/plat-samsung/setup-mipiphy.c
@@ -14,24 +14,18 @@
 #include <linux/spinlock.h>
 #include <mach/regs-clock.h>
 
-static int __s5p_mipi_phy_control(struct platform_device *pdev,
-                                  bool on, u32 reset)
+static int __s5p_mipi_phy_control(int id, bool on, u32 reset)
 {
         static DEFINE_SPINLOCK(lock);
         void __iomem *addr;
         unsigned long flags;
-        int pid;
         u32 cfg;
 
-        if (!pdev)
+        id = max(0, id);
+        if (id > 1)
                 return -EINVAL;
 
-        pid = (pdev->id == -1) ? 0 : pdev->id;
-
-        if (pid != 0 && pid != 1)
-                return -EINVAL;
-
-        addr = S5P_MIPI_DPHY_CONTROL(pid);
+        addr = S5P_MIPI_DPHY_CONTROL(id);
 
         spin_lock_irqsave(&lock, flags);
 
@@ -52,12 +46,12 @@ static int __s5p_mipi_phy_control(struct platform_device *pdev,
         return 0;
 }
 
-int s5p_csis_phy_enable(struct platform_device *pdev, bool on)
+int s5p_csis_phy_enable(int id, bool on)
 {
-        return __s5p_mipi_phy_control(pdev, on, S5P_MIPI_DPHY_SRESETN);
+        return __s5p_mipi_phy_control(id, on, S5P_MIPI_DPHY_SRESETN);
 }
 
 int s5p_dsim_phy_enable(struct platform_device *pdev, bool on)
 {
-        return __s5p_mipi_phy_control(pdev, on, S5P_MIPI_DPHY_MRESETN);
+        return __s5p_mipi_phy_control(pdev->id, on, S5P_MIPI_DPHY_MRESETN);
 }
diff --git a/drivers/media/dvb-core/dvb_frontend.c b/drivers/media/dvb-core/dvb_frontend.c
index 8f58f241c10d..7e92793260f0 100644
--- a/drivers/media/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb-core/dvb_frontend.c
@@ -966,6 +966,8 @@ static int dvb_frontend_clear_cache(struct dvb_frontend *fe)
                 break;
         }
 
+        c->lna = LNA_AUTO;
+
         return 0;
 }
 
@@ -1054,6 +1056,8 @@ static struct dtv_cmds_h dtv_cmds[DTV_MAX_COMMAND + 1] = {
         _DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_B, 0, 0),
         _DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_C, 0, 0),
         _DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_D, 0, 0),
+
+        _DTV_CMD(DTV_LNA, 0, 0),
 };
 
 static void dtv_property_dump(struct dvb_frontend *fe, struct dtv_property *tvp)
@@ -1440,6 +1444,10 @@ static int dtv_property_process_get(struct dvb_frontend *fe,
                 tvp->u.data = fe->dtv_property_cache.atscmh_sccc_code_mode_d;
                 break;
 
+        case DTV_LNA:
+                tvp->u.data = c->lna;
+                break;
+
         default:
                 return -EINVAL;
         }
@@ -1731,10 +1739,6 @@ static int dtv_property_process_set(struct dvb_frontend *fe,
         case DTV_INTERLEAVING:
                 c->interleaving = tvp->u.data;
                 break;
-        case DTV_LNA:
-                if (fe->ops.set_lna)
-                        r = fe->ops.set_lna(fe, tvp->u.data);
-                break;
 
         /* ISDB-T Support here */
         case DTV_ISDBT_PARTIAL_RECEPTION:
@@ -1806,6 +1810,12 @@ static int dtv_property_process_set(struct dvb_frontend *fe,
                 fe->dtv_property_cache.atscmh_rs_frame_ensemble = tvp->u.data;
                 break;
 
+        case DTV_LNA:
+                c->lna = tvp->u.data;
+                if (fe->ops.set_lna)
+                        r = fe->ops.set_lna(fe);
+                break;
+
         default:
                 return -EINVAL;
         }
@@ -2309,7 +2319,7 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
                 fepriv->tune_mode_flags = (unsigned long) parg;
                 err = 0;
                 break;
-        };
+        }
 
         return err;
 }
diff --git a/drivers/media/dvb-core/dvb_frontend.h b/drivers/media/dvb-core/dvb_frontend.h
index 44a445cee74f..97112cd88a17 100644
--- a/drivers/media/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb-core/dvb_frontend.h
@@ -303,7 +303,7 @@ struct dvb_frontend_ops {
         int (*dishnetwork_send_legacy_command)(struct dvb_frontend* fe, unsigned long cmd);
         int (*i2c_gate_ctrl)(struct dvb_frontend* fe, int enable);
         int (*ts_bus_ctrl)(struct dvb_frontend* fe, int acquire);
-        int (*set_lna)(struct dvb_frontend *, int);
+        int (*set_lna)(struct dvb_frontend *);
 
         /* These callbacks are for devices that implement their own
          * tuning algorithms, rather than a simple swzigzag
@@ -391,6 +391,8 @@ struct dtv_frontend_properties {
         u8                      atscmh_sccc_code_mode_b;
         u8                      atscmh_sccc_code_mode_c;
         u8                      atscmh_sccc_code_mode_d;
+
+        u32                     lna;
 };
 
 struct dvb_frontend {
diff --git a/drivers/media/dvb-frontends/a8293.c b/drivers/media/dvb-frontends/a8293.c
index cff44a389b40..74fbb5d58bed 100644
--- a/drivers/media/dvb-frontends/a8293.c
+++ b/drivers/media/dvb-frontends/a8293.c
@@ -90,7 +90,7 @@ static int a8293_set_voltage(struct dvb_frontend *fe,
         default:
                 ret = -EINVAL;
                 goto err;
-        };
+        }
 
         ret = a8293_wr(priv, &priv->reg[0], 1);
         if (ret)
diff --git a/drivers/media/dvb-frontends/af9013.c b/drivers/media/dvb-frontends/af9013.c
index e9f04a36577b..a204f2828820 100644
--- a/drivers/media/dvb-frontends/af9013.c
+++ b/drivers/media/dvb-frontends/af9013.c
@@ -241,7 +241,7 @@ static int af9013_set_gpio(struct af9013_state *state, u8 gpio, u8 gpioval)
                                 KBUILD_MODNAME, gpio);
                 ret = -EINVAL;
                 goto err;
-        };
+        }
 
         switch (gpio) {
         case 0:
@@ -253,7 +253,7 @@ static int af9013_set_gpio(struct af9013_state *state, u8 gpio, u8 gpioval)
         default:
                 pos = 4;
                 break;
-        };
+        }
 
         ret = af9013_wr_reg_bits(state, addr, pos, 4, gpioval);
         if (ret)
@@ -726,7 +726,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
         default:
                 dev_dbg(&state->i2c->dev, "%s: invalid hierarchy\n", __func__);
                 auto_mode = 1;
-        };
+        }
 
         switch (c->modulation) {
         case QAM_AUTO:
diff --git a/drivers/media/dvb-frontends/af9033.c b/drivers/media/dvb-frontends/af9033.c
index 8162d939c4b2..464ad878490b 100644
--- a/drivers/media/dvb-frontends/af9033.c
+++ b/drivers/media/dvb-frontends/af9033.c
@@ -408,7 +408,7 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
 {
         struct af9033_state *state = fe->demodulator_priv;
         struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-        int ret, i, spec_inv;
+        int ret, i, spec_inv, sampling_freq;
         u8 tmp, buf[3], bandwidth_reg_val;
         u32 if_frequency, freq_cw, adc_freq;
 
@@ -465,18 +465,20 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
                 else
                         if_frequency = 0;
 
-                while (if_frequency > (adc_freq / 2))
-                        if_frequency -= adc_freq;
+                sampling_freq = if_frequency;
 
-                if (if_frequency >= 0)
+                while (sampling_freq > (adc_freq / 2))
+                        sampling_freq -= adc_freq;
+
+                if (sampling_freq >= 0)
                         spec_inv *= -1;
                 else
-                        if_frequency *= -1;
+                        sampling_freq *= -1;
 
-                freq_cw = af9033_div(state, if_frequency, adc_freq, 23ul);
+                freq_cw = af9033_div(state, sampling_freq, adc_freq, 23ul);
 
                 if (spec_inv == -1)
-                        freq_cw *= -1;
+                        freq_cw = 0x800000 - freq_cw;
 
                 /* get adc multiplies */
                 ret = af9033_rd_reg(state, 0x800045, &tmp);
diff --git a/drivers/media/dvb-frontends/bcm3510.c b/drivers/media/dvb-frontends/bcm3510.c
index 033cd7ad3ca2..1b77909c0c71 100644
--- a/drivers/media/dvb-frontends/bcm3510.c
+++ b/drivers/media/dvb-frontends/bcm3510.c
@@ -527,7 +527,7 @@ static int bcm3510_set_frontend(struct dvb_frontend *fe)
                         cmd.ACQUIRE1.IF_FREQ = 0x0;
                 default:
                         return -EINVAL;
-        };
+        }
         cmd.ACQUIRE0.OFFSET = 0;
         cmd.ACQUIRE0.NTSCSWEEP = 1;
         cmd.ACQUIRE0.FA = 1;
diff --git a/drivers/media/dvb-frontends/cx24110.c b/drivers/media/dvb-frontends/cx24110.c
index 3180f5b2a6a6..0cd6927e654c 100644
--- a/drivers/media/dvb-frontends/cx24110.c
+++ b/drivers/media/dvb-frontends/cx24110.c
@@ -218,7 +218,7 @@ static int cx24110_set_fec (struct cx24110_state* state, fe_code_rate_t fec)
            } else
                    return -EOPNOTSUPP;
 /* fixme (low): which is the correct return code? */
-        };
+        }
         return 0;
 }
 
@@ -275,7 +275,7 @@ static int cx24110_set_symbolrate (struct cx24110_state* state, u32 srate)
                 cx24110_writereg(state,0x07,tmp|0x3);
                 cx24110_writereg(state,0x06,0x78);
                 fclk=90999000UL;
-        };
+        }
         dprintk("cx24110 debug: fclk %d Hz\n",fclk);
         /* we need to divide two integers with approx. 27 bits in 32 bit
            arithmetic giving a 25 bit result */
@@ -362,7 +362,7 @@ static int cx24110_initfe(struct dvb_frontend* fe)
 
         for(i = 0; i < ARRAY_SIZE(cx24110_regdata); i++) {
                 cx24110_writereg(state, cx24110_regdata[i].reg, cx24110_regdata[i].data);
-        };
+        }
 
         return 0;
 }
diff --git a/drivers/media/dvb-frontends/cxd2820r_core.c b/drivers/media/dvb-frontends/cxd2820r_core.c
index 42648643693e..9b658c1cf39a 100644
--- a/drivers/media/dvb-frontends/cxd2820r_core.c
+++ b/drivers/media/dvb-frontends/cxd2820r_core.c
@@ -688,7 +688,7 @@ struct dvb_frontend *cxd2820r_attach(const struct cxd2820r_config *cfg,
 {
         struct cxd2820r_priv *priv;
         int ret;
-        u8 tmp, gpio[GPIO_COUNT];
+        u8 tmp;
 
         priv = kzalloc(sizeof(struct cxd2820r_priv), GFP_KERNEL);
         if (!priv) {
@@ -735,6 +735,7 @@ struct dvb_frontend *cxd2820r_attach(const struct cxd2820r_config *cfg,
                  * Use static GPIO configuration if GPIOLIB is undefined.
                  * This is fallback condition.
                  */
+                u8 gpio[GPIO_COUNT];
                 gpio[0] = (*gpio_chip_base >> 0) & 0x07;
                 gpio[1] = (*gpio_chip_base >> 3) & 0x07;
                 gpio[2] = 0;
diff --git a/drivers/media/dvb-frontends/drxd_hard.c b/drivers/media/dvb-frontends/drxd_hard.c
index f380eb43e9d5..6d9853750d2b 100644
--- a/drivers/media/dvb-frontends/drxd_hard.c
+++ b/drivers/media/dvb-frontends/drxd_hard.c
@@ -991,7 +991,7 @@ static int HI_Command(struct drxd_state *state, u16 cmd, u16 * pResult)
                 if (nrRetries > DRXD_MAX_RETRIES) {
                         status = -1;
                         break;
-                };
+                }
                 status = Read16(state, HI_RA_RAM_SRV_CMD__A, &waitCmd, 0);
         } while (waitCmd != 0);
 
diff --git a/drivers/media/dvb-frontends/ds3000.c b/drivers/media/dvb-frontends/ds3000.c
index 4c8ac2657c4a..5b639087ce45 100644
--- a/drivers/media/dvb-frontends/ds3000.c
+++ b/drivers/media/dvb-frontends/ds3000.c
@@ -30,6 +30,7 @@
 #include "ds3000.h"
 
 static int debug;
+static int force_fw_upload;
 
 #define dprintk(args...) \
         do { \
@@ -392,11 +393,13 @@ static int ds3000_firmware_ondemand(struct dvb_frontend *fe)
 
         dprintk("%s()\n", __func__);
 
-        if (ds3000_readreg(state, 0xb2) <= 0)
+        ret = ds3000_readreg(state, 0xb2);
+        if (ret < 0)
                 return ret;
 
-        if (state->skip_fw_load)
-                return 0;
+        if (state->skip_fw_load || !force_fw_upload)
+                return 0;       /* Firmware already uploaded, skipping */
+
         /* Load firmware */
         /* request the firmware, this will block until someone uploads it */
         printk(KERN_INFO "%s: Waiting for firmware upload (%s)...\n", __func__,
@@ -1306,6 +1309,9 @@ static struct dvb_frontend_ops ds3000_ops = {
 module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "Activates frontend debugging (default:0)");
 
+module_param(force_fw_upload, int, 0644);
+MODULE_PARM_DESC(force_fw_upload, "Force firmware upload (default:0)");
+
 MODULE_DESCRIPTION("DVB Frontend module for Montage Technology "
                         "DS3000/TS2020 hardware");
 MODULE_AUTHOR("Konstantin Dimitrov");
diff --git a/drivers/media/dvb-frontends/dvb_dummy_fe.c b/drivers/media/dvb-frontends/dvb_dummy_fe.c
index dcfc902c8678..d5acc304786b 100644
--- a/drivers/media/dvb-frontends/dvb_dummy_fe.c
+++ b/drivers/media/dvb-frontends/dvb_dummy_fe.c
@@ -121,16 +121,13 @@ struct dvb_frontend* dvb_dummy_fe_ofdm_attach(void)
 
         /* allocate memory for the internal state */
         state = kzalloc(sizeof(struct dvb_dummy_fe_state), GFP_KERNEL);
-        if (state == NULL) goto error;
+        if (!state)
+                return NULL;
 
         /* create dvb_frontend */
         memcpy(&state->frontend.ops, &dvb_dummy_fe_ofdm_ops, sizeof(struct dvb_frontend_ops));
         state->frontend.demodulator_priv = state;
         return &state->frontend;
-
-error:
-        kfree(state);
-        return NULL;
 }
 
 static struct dvb_frontend_ops dvb_dummy_fe_qpsk_ops;
@@ -141,16 +138,13 @@ struct dvb_frontend *dvb_dummy_fe_qpsk_attach(void)
 
         /* allocate memory for the internal state */
         state = kzalloc(sizeof(struct dvb_dummy_fe_state), GFP_KERNEL);
-        if (state == NULL) goto error;
+        if (!state)
+                return NULL;
 
         /* create dvb_frontend */
         memcpy(&state->frontend.ops, &dvb_dummy_fe_qpsk_ops, sizeof(struct dvb_frontend_ops));
         state->frontend.demodulator_priv = state;
         return &state->frontend;
-
-error:
-        kfree(state);
-        return NULL;
 }
 
 static struct dvb_frontend_ops dvb_dummy_fe_qam_ops;
@@ -161,16 +155,13 @@ struct dvb_frontend *dvb_dummy_fe_qam_attach(void)
 
         /* allocate memory for the internal state */
         state = kzalloc(sizeof(struct dvb_dummy_fe_state), GFP_KERNEL);
-        if (state == NULL) goto error;
+        if (!state)
+                return NULL;
 
         /* create dvb_frontend */
         memcpy(&state->frontend.ops, &dvb_dummy_fe_qam_ops, sizeof(struct dvb_frontend_ops));
         state->frontend.demodulator_priv = state;
         return &state->frontend;
-
-error:
-        kfree(state);
-        return NULL;
 }
 
 static struct dvb_frontend_ops dvb_dummy_fe_ofdm_ops = {
diff --git a/drivers/media/dvb-frontends/isl6405.c b/drivers/media/dvb-frontends/isl6405.c
index 33d33f4d8867..0c642a5bf823 100644
--- a/drivers/media/dvb-frontends/isl6405.c
+++ b/drivers/media/dvb-frontends/isl6405.c
@@ -77,7 +77,7 @@ static int isl6405_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage
                         break;
                 default:
                         return -EINVAL;
-                };
+                }
         }
         isl6405->config |= isl6405->override_or;
         isl6405->config &= isl6405->override_and;
diff --git a/drivers/media/dvb-frontends/isl6421.c b/drivers/media/dvb-frontends/isl6421.c
index 684c8ec166cb..0cb3f0f74c9c 100644
--- a/drivers/media/dvb-frontends/isl6421.c
+++ b/drivers/media/dvb-frontends/isl6421.c
@@ -63,7 +63,7 @@ static int isl6421_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         isl6421->config |= isl6421->override_or;
         isl6421->config &= isl6421->override_and;
diff --git a/drivers/media/dvb-frontends/itd1000.c b/drivers/media/dvb-frontends/itd1000.c
index 316457584fe7..c1c3400b2173 100644
--- a/drivers/media/dvb-frontends/itd1000.c
+++ b/drivers/media/dvb-frontends/itd1000.c
@@ -231,7 +231,7 @@ static void itd1000_set_lo(struct itd1000_state *state, u32 freq_khz)
         state->frequency = ((plln * 1000) + (pllf * 1000)/1048576) * 2*FREF;
         itd_dbg("frequency: %dkHz (wanted) %dkHz (set), PLLF = %d, PLLN = %d\n", freq_khz, state->frequency, pllf, plln);
 
-        itd1000_write_reg(state, PLLNH, 0x80); /* PLLNH */;
+        itd1000_write_reg(state, PLLNH, 0x80); /* PLLNH */
         itd1000_write_reg(state, PLLNL, plln & 0xff);
         itd1000_write_reg(state, PLLFH, (itd1000_read_reg(state, PLLFH) & 0xf0) | ((pllf >> 16) & 0x0f));
         itd1000_write_reg(state, PLLFM, (pllf >> 8) & 0xff);
diff --git a/drivers/media/dvb-frontends/lg2160.c b/drivers/media/dvb-frontends/lg2160.c
index cc11260e99df..5fd14f840ab0 100644
--- a/drivers/media/dvb-frontends/lg2160.c
+++ b/drivers/media/dvb-frontends/lg2160.c
@@ -1421,8 +1421,8 @@ struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
                config ? config->i2c_addr : 0);
 
         state = kzalloc(sizeof(struct lg216x_state), GFP_KERNEL);
-        if (state == NULL)
-                goto fail;
+        if (!state)
+                return NULL;
 
         state->cfg = config;
         state->i2c_adap = i2c_adap;
@@ -1449,10 +1449,6 @@ struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
         state->frontend.dtv_property_cache.atscmh_parade_id = 1;
 
         return &state->frontend;
-fail:
-        lg_warn("unable to detect LG216x hardware\n");
-        kfree(state);
-        return NULL;
 }
 EXPORT_SYMBOL(lg2160_attach);
 
diff --git a/drivers/media/dvb-frontends/lnbp21.c b/drivers/media/dvb-frontends/lnbp21.c
index 13437259eeac..f3ba7b5faa2e 100644
--- a/drivers/media/dvb-frontends/lnbp21.c
+++ b/drivers/media/dvb-frontends/lnbp21.c
@@ -65,7 +65,7 @@ static int lnbp21_set_voltage(struct dvb_frontend *fe,
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         lnbp21->config |= lnbp21->override_or;
         lnbp21->config &= lnbp21->override_and;
@@ -108,7 +108,7 @@ static int lnbp21_set_tone(struct dvb_frontend *fe,
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         lnbp21->config |= lnbp21->override_or;
         lnbp21->config &= lnbp21->override_and;
diff --git a/drivers/media/dvb-frontends/lnbp22.c b/drivers/media/dvb-frontends/lnbp22.c
index 84ad0390a4a1..c463da7f6dcc 100644
--- a/drivers/media/dvb-frontends/lnbp22.c
+++ b/drivers/media/dvb-frontends/lnbp22.c
@@ -73,7 +73,7 @@ static int lnbp22_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         dprintk(1, "%s: 0x%02x)\n", __func__, lnbp22->config[3]);
         return (i2c_transfer(lnbp22->i2c, &msg, 1) == 1) ? 0 : -EIO;
diff --git a/drivers/media/dvb-frontends/s5h1432.c b/drivers/media/dvb-frontends/s5h1432.c
index 8352ce1c9556..6ec16a243741 100644
--- a/drivers/media/dvb-frontends/s5h1432.c
+++ b/drivers/media/dvb-frontends/s5h1432.c
@@ -351,8 +351,8 @@ struct dvb_frontend *s5h1432_attach(const struct s5h1432_config *config,
         printk(KERN_INFO " Enter s5h1432_attach(). attach success!\n");
         /* allocate memory for the internal state */
         state = kmalloc(sizeof(struct s5h1432_state), GFP_KERNEL);
-        if (state == NULL)
-                goto error;
+        if (!state)
+                return NULL;
 
         /* setup the state */
         state->config = config;
@@ -367,10 +367,6 @@ struct dvb_frontend *s5h1432_attach(const struct s5h1432_config *config,
         state->frontend.demodulator_priv = state;
 
         return &state->frontend;
-
-error:
-        kfree(state);
-        return NULL;
 }
 EXPORT_SYMBOL(s5h1432_attach);
 
diff --git a/drivers/media/dvb-frontends/s921.c b/drivers/media/dvb-frontends/s921.c
index cd2288c07147..a271ac3eaec0 100644
--- a/drivers/media/dvb-frontends/s921.c
+++ b/drivers/media/dvb-frontends/s921.c
@@ -487,9 +487,9 @@ struct dvb_frontend *s921_attach(const struct s921_config *config,
                 kzalloc(sizeof(struct s921_state), GFP_KERNEL);
 
         dprintk("\n");
-        if (state == NULL) {
+        if (!state) {
                 rc("Unable to kzalloc\n");
-                goto rcor;
+                return NULL;
         }
 
         /* setup the state */
@@ -502,11 +502,6 @@ struct dvb_frontend *s921_attach(const struct s921_config *config,
         state->frontend.demodulator_priv = state;
 
         return &state->frontend;
-
-rcor:
-        kfree(state);
-
-        return NULL;
 }
 EXPORT_SYMBOL(s921_attach);
 
diff --git a/drivers/media/dvb-frontends/si21xx.c b/drivers/media/dvb-frontends/si21xx.c
index a68a64800df7..73b47cc6a13b 100644
--- a/drivers/media/dvb-frontends/si21xx.c
+++ b/drivers/media/dvb-frontends/si21xx.c
@@ -343,7 +343,7 @@ static int si21xx_wait_diseqc_idle(struct si21xx_state *state, int timeout)
                         return -ETIMEDOUT;
                 }
                 msleep(10);
-        };
+        }
 
         return 0;
 }
@@ -472,7 +472,7 @@ static int si21xx_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t volt)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 }
 
 static int si21xx_init(struct dvb_frontend *fe)
diff --git a/drivers/media/dvb-frontends/sp8870.c b/drivers/media/dvb-frontends/sp8870.c
index e37274c8f14e..2aa8ef76eba2 100644
--- a/drivers/media/dvb-frontends/sp8870.c
+++ b/drivers/media/dvb-frontends/sp8870.c
@@ -188,7 +188,7 @@ static int configure_reg0xc05 (struct dtv_frontend_properties *p, u16 *reg0xc05)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         switch (p->hierarchy) {
         case HIERARCHY_NONE:
@@ -207,7 +207,7 @@ static int configure_reg0xc05 (struct dtv_frontend_properties *p, u16 *reg0xc05)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         switch (p->code_rate_HP) {
         case FEC_1_2:
@@ -229,7 +229,7 @@ static int configure_reg0xc05 (struct dtv_frontend_properties *p, u16 *reg0xc05)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         if (known_parameters)
                 *reg0xc05 |= (2 << 1);  /* use specified parameters */
diff --git a/drivers/media/dvb-frontends/sp887x.c b/drivers/media/dvb-frontends/sp887x.c
index f4096ccb226e..1bb81b5ae6e0 100644
--- a/drivers/media/dvb-frontends/sp887x.c
+++ b/drivers/media/dvb-frontends/sp887x.c
@@ -229,7 +229,7 @@ static int configure_reg0xc05(struct dtv_frontend_properties *p, u16 *reg0xc05)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         switch (p->hierarchy) {
         case HIERARCHY_NONE:
@@ -248,7 +248,7 @@ static int configure_reg0xc05(struct dtv_frontend_properties *p, u16 *reg0xc05)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         switch (p->code_rate_HP) {
         case FEC_1_2:
@@ -270,7 +270,7 @@ static int configure_reg0xc05(struct dtv_frontend_properties *p, u16 *reg0xc05)
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         if (known_parameters)
                 *reg0xc05 |= (2 << 1);  /* use specified parameters */
diff --git a/drivers/media/dvb-frontends/stb6100.c b/drivers/media/dvb-frontends/stb6100.c
index 2e93e65d2cdb..45f9523f968f 100644
--- a/drivers/media/dvb-frontends/stb6100.c
+++ b/drivers/media/dvb-frontends/stb6100.c
@@ -575,8 +575,8 @@ struct dvb_frontend *stb6100_attach(struct dvb_frontend *fe,
         struct stb6100_state *state = NULL;
 
         state = kzalloc(sizeof (struct stb6100_state), GFP_KERNEL);
-        if (state == NULL)
-                goto error;
+        if (!state)
+                return NULL;
 
         state->config           = config;
         state->i2c              = i2c;
@@ -587,10 +587,6 @@ struct dvb_frontend *stb6100_attach(struct dvb_frontend *fe,
 
         printk("%s: Attaching STB6100 \n", __func__);
         return fe;
-
-error:
-        kfree(state);
-        return NULL;
 }
 
 static int stb6100_release(struct dvb_frontend *fe)
diff --git a/drivers/media/dvb-frontends/stv0299.c b/drivers/media/dvb-frontends/stv0299.c
index 057b5f8effc0..92a6075cd82f 100644
--- a/drivers/media/dvb-frontends/stv0299.c
+++ b/drivers/media/dvb-frontends/stv0299.c
@@ -199,7 +199,7 @@ static int stv0299_wait_diseqc_fifo (struct stv0299_state* state, int timeout)
                         return -ETIMEDOUT;
                 }
                 msleep(10);
-        };
+        }
 
         return 0;
 }
@@ -216,7 +216,7 @@ static int stv0299_wait_diseqc_idle (struct stv0299_state* state, int timeout)
                         return -ETIMEDOUT;
                 }
                 msleep(10);
-        };
+        }
 
         return 0;
 }
@@ -387,7 +387,7 @@ static int stv0299_set_voltage (struct dvb_frontend* fe, fe_sec_voltage_t voltag
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         if (state->config->op0_off)
                 reg0x0c &= ~0x10;
diff --git a/drivers/media/dvb-frontends/stv0900_core.c b/drivers/media/dvb-frontends/stv0900_core.c
index 7f1badaf0d03..262dfa503c2a 100644
--- a/drivers/media/dvb-frontends/stv0900_core.c
+++ b/drivers/media/dvb-frontends/stv0900_core.c
@@ -1552,8 +1552,8 @@ static int stv0900_status(struct stv0900_internal *intp,
                 bitrate = (stv0900_get_mclk_freq(intp, intp->quartz)/1000000)
                         * (tsbitrate1_val << 8 | tsbitrate0_val);
                 bitrate /= 16384;
-                dprintk("TS bitrate = %d Mbit/sec \n", bitrate);
-        };
+                dprintk("TS bitrate = %d Mbit/sec\n", bitrate);
+        }
 
         return locked;
 }
diff --git a/drivers/media/dvb-frontends/tda665x.c b/drivers/media/dvb-frontends/tda665x.c
index 2c1c759a4f42..63cc12378d9a 100644
--- a/drivers/media/dvb-frontends/tda665x.c
+++ b/drivers/media/dvb-frontends/tda665x.c
@@ -228,8 +228,8 @@ struct dvb_frontend *tda665x_attach(struct dvb_frontend *fe,
         struct dvb_tuner_info *info;
 
         state = kzalloc(sizeof(struct tda665x_state), GFP_KERNEL);
-        if (state == NULL)
-                goto exit;
+        if (!state)
+                return NULL;
 
         state->config           = config;
         state->i2c              = i2c;
@@ -246,10 +246,6 @@ struct dvb_frontend *tda665x_attach(struct dvb_frontend *fe,
         printk(KERN_DEBUG "%s: Attaching TDA665x (%s) tuner\n", __func__, info->name);
 
         return fe;
-
-exit:
-        kfree(state);
-        return NULL;
 }
 EXPORT_SYMBOL(tda665x_attach);
 
diff --git a/drivers/media/dvb-frontends/tda8083.c b/drivers/media/dvb-frontends/tda8083.c
index 15912c96926a..9d08350fe4b0 100644
--- a/drivers/media/dvb-frontends/tda8083.c
+++ b/drivers/media/dvb-frontends/tda8083.c
@@ -175,7 +175,7 @@ static void tda8083_wait_diseqc_fifo (struct tda8083_state* state, int timeout)
                !(tda8083_readreg(state, 0x02) & 0x80))
         {
                 msleep(50);
-        };
+        }
 }
 
 static int tda8083_set_tone (struct tda8083_state* state, fe_sec_tone_mode_t tone)
@@ -215,7 +215,7 @@ static int tda8083_send_diseqc_burst (struct tda8083_state* state, fe_sec_mini_c
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         tda8083_wait_diseqc_fifo (state, 100);
 
diff --git a/drivers/media/i2c/cx25840/cx25840-core.c b/drivers/media/i2c/cx25840/cx25840-core.c
index d8eac3e30a7e..2cee69e34184 100644
--- a/drivers/media/i2c/cx25840/cx25840-core.c
+++ b/drivers/media/i2c/cx25840/cx25840-core.c
@@ -599,7 +599,7 @@ static void cx23885_initialize(struct i2c_client *client)
                 cx25840_write4(client, 0x114, 0x01bf0c9e);
                 cx25840_write4(client, 0x110, 0x000a030c);
                 break;
-        };
+        }
 
         /* ADC2 input select */
         cx25840_write(client, 0x102, 0x10);
diff --git a/drivers/media/i2c/m5mols/m5mols.h b/drivers/media/i2c/m5mols/m5mols.h
index 86c815be348c..90a6c520f115 100644
--- a/drivers/media/i2c/m5mols/m5mols.h
+++ b/drivers/media/i2c/m5mols/m5mols.h
@@ -16,9 +16,17 @@
 #ifndef M5MOLS_H
 #define M5MOLS_H
 
+#include <linux/sizes.h>
 #include <media/v4l2-subdev.h>
 #include "m5mols_reg.h"
 
+
+/* An amount of data transmitted in addition to the value
+ * determined by CAPP_JPEG_SIZE_MAX register.
+ */
+#define M5MOLS_JPEG_TAGS_SIZE           0x20000
+#define M5MOLS_MAIN_JPEG_SIZE_MAX       (5 * SZ_1M)
+
 extern int m5mols_debug;
 
 enum m5mols_restype {
@@ -67,12 +75,14 @@ struct m5mols_exif {
 /**
  * struct m5mols_capture - Structure for the capture capability
  * @exif: EXIF information
+ * @buf_size: internal JPEG frame buffer size, in bytes
  * @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;
+        unsigned int buf_size;
         u32 main;
         u32 thumb;
         u32 total;
diff --git a/drivers/media/i2c/m5mols/m5mols_capture.c b/drivers/media/i2c/m5mols/m5mols_capture.c
index cb243bd278ce..ab34ccedf31e 100644
--- a/drivers/media/i2c/m5mols/m5mols_capture.c
+++ b/drivers/media/i2c/m5mols/m5mols_capture.c
@@ -105,6 +105,7 @@ static int m5mols_capture_info(struct m5mols_info *info)
 
 int m5mols_start_capture(struct m5mols_info *info)
 {
+        unsigned int framesize = info->cap.buf_size - M5MOLS_JPEG_TAGS_SIZE;
         struct v4l2_subdev *sd = &info->sd;
         int ret;
 
@@ -121,6 +122,8 @@ int m5mols_start_capture(struct m5mols_info *info)
         if (!ret)
                 ret = m5mols_write(sd, CAPP_MAIN_IMAGE_SIZE, info->resolution);
         if (!ret)
+                ret = m5mols_write(sd, CAPP_JPEG_SIZE_MAX, framesize);
+        if (!ret)
                 ret = m5mols_set_mode(info, REG_CAPTURE);
         if (!ret)
                 /* Wait until a frame is captured to ISP internal memory */
diff --git a/drivers/media/i2c/m5mols/m5mols_core.c b/drivers/media/i2c/m5mols/m5mols_core.c
index 2f490ef26c38..8131d651de9e 100644
--- a/drivers/media/i2c/m5mols/m5mols_core.c
+++ b/drivers/media/i2c/m5mols/m5mols_core.c
@@ -599,6 +599,51 @@ static int m5mols_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
         return ret;
 }
 
+static int m5mols_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+                                 struct v4l2_mbus_frame_desc *fd)
+{
+        struct m5mols_info *info = to_m5mols(sd);
+
+        if (pad != 0 || fd == NULL)
+                return -EINVAL;
+
+        mutex_lock(&info->lock);
+        /*
+         * .get_frame_desc is only used for compressed formats,
+         * thus we always return the capture frame parameters here.
+         */
+        fd->entry[0].length = info->cap.buf_size;
+        fd->entry[0].pixelcode = info->ffmt[M5MOLS_RESTYPE_CAPTURE].code;
+        mutex_unlock(&info->lock);
+
+        fd->entry[0].flags = V4L2_MBUS_FRAME_DESC_FL_LEN_MAX;
+        fd->num_entries = 1;
+
+        return 0;
+}
+
+static int m5mols_set_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+                                 struct v4l2_mbus_frame_desc *fd)
+{
+        struct m5mols_info *info = to_m5mols(sd);
+        struct v4l2_mbus_framefmt *mf = &info->ffmt[M5MOLS_RESTYPE_CAPTURE];
+
+        if (pad != 0 || fd == NULL)
+                return -EINVAL;
+
+        fd->entry[0].flags = V4L2_MBUS_FRAME_DESC_FL_LEN_MAX;
+        fd->num_entries = 1;
+        fd->entry[0].length = clamp_t(u32, fd->entry[0].length,
+                                      mf->width * mf->height,
+                                      M5MOLS_MAIN_JPEG_SIZE_MAX);
+        mutex_lock(&info->lock);
+        info->cap.buf_size = fd->entry[0].length;
+        mutex_unlock(&info->lock);
+
+        return 0;
+}
+
+
 static int m5mols_enum_mbus_code(struct v4l2_subdev *sd,
                                  struct v4l2_subdev_fh *fh,
                                  struct v4l2_subdev_mbus_code_enum *code)
@@ -615,6 +660,8 @@ 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,
+        .get_frame_desc = m5mols_get_frame_desc,
+        .set_frame_desc = m5mols_set_frame_desc,
 };
 
 /**
diff --git a/drivers/media/i2c/m5mols/m5mols_reg.h b/drivers/media/i2c/m5mols/m5mols_reg.h
index 14d4be72aeff..58d8027508df 100644
--- a/drivers/media/i2c/m5mols/m5mols_reg.h
+++ b/drivers/media/i2c/m5mols/m5mols_reg.h
@@ -310,6 +310,7 @@
 #define REG_JPEG                0x10
 
 #define CAPP_MAIN_IMAGE_SIZE    I2C_REG(CAT_CAPT_PARM, 0x01, 1)
+#define CAPP_JPEG_SIZE_MAX      I2C_REG(CAT_CAPT_PARM, 0x0f, 4)
 #define CAPP_JPEG_RATIO         I2C_REG(CAT_CAPT_PARM, 0x17, 1)
 
 #define CAPP_MCC_MODE           I2C_REG(CAT_CAPT_PARM, 0x1d, 1)
diff --git a/drivers/media/i2c/mt9p031.c b/drivers/media/i2c/mt9p031.c
index 2c0f4077c491..e32833262d32 100644
--- a/drivers/media/i2c/mt9p031.c
+++ b/drivers/media/i2c/mt9p031.c
@@ -574,7 +574,6 @@ static int mt9p031_set_crop(struct v4l2_subdev *subdev,
  * V4L2 subdev control operations
  */
 
-#define V4L2_CID_TEST_PATTERN           (V4L2_CID_USER_BASE | 0x1001)
 #define V4L2_CID_BLC_AUTO               (V4L2_CID_USER_BASE | 0x1002)
 #define V4L2_CID_BLC_TARGET_LEVEL       (V4L2_CID_USER_BASE | 0x1003)
 #define V4L2_CID_BLC_ANALOG_OFFSET      (V4L2_CID_USER_BASE | 0x1004)
@@ -740,18 +739,6 @@ static const char * const mt9p031_test_pattern_menu[] = {
 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
         {
                 .ops            = &mt9p031_ctrl_ops,
-                .id             = V4L2_CID_TEST_PATTERN,
-                .type           = V4L2_CTRL_TYPE_MENU,
-                .name           = "Test Pattern",
-                .min            = 0,
-                .max            = ARRAY_SIZE(mt9p031_test_pattern_menu) - 1,
-                .step           = 0,
-                .def            = 0,
-                .flags          = 0,
-                .menu_skip_mask = 0,
-                .qmenu          = mt9p031_test_pattern_menu,
-        }, {
-                .ops            = &mt9p031_ctrl_ops,
                 .id             = V4L2_CID_BLC_AUTO,
                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
                 .name           = "BLC, Auto",
@@ -950,7 +937,7 @@ static int mt9p031_probe(struct i2c_client *client,
         mt9p031->model = did->driver_data;
         mt9p031->reset = -1;
 
-        v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 5);
+        v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
 
         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
                           V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
@@ -966,6 +953,10 @@ static int mt9p031_probe(struct i2c_client *client,
         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
                           V4L2_CID_PIXEL_RATE, pdata->target_freq,
                           pdata->target_freq, 1, pdata->target_freq);
+        v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
+                          V4L2_CID_TEST_PATTERN,
+                          ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
+                          0, mt9p031_test_pattern_menu);
 
         for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
                 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
diff --git a/drivers/media/i2c/mt9t001.c b/drivers/media/i2c/mt9t001.c
index 6d343adf891d..2e189d8b71bb 100644
--- a/drivers/media/i2c/mt9t001.c
+++ b/drivers/media/i2c/mt9t001.c
@@ -371,7 +371,7 @@ static int mt9t001_set_crop(struct v4l2_subdev *subdev,
  * V4L2 subdev control operations
  */
 
-#define V4L2_CID_TEST_PATTERN           (V4L2_CID_USER_BASE | 0x1001)
+#define V4L2_CID_TEST_PATTERN_COLOR     (V4L2_CID_USER_BASE | 0x1001)
 #define V4L2_CID_BLACK_LEVEL_AUTO       (V4L2_CID_USER_BASE | 0x1002)
 #define V4L2_CID_BLACK_LEVEL_OFFSET     (V4L2_CID_USER_BASE | 0x1003)
 #define V4L2_CID_BLACK_LEVEL_CALIBRATE  (V4L2_CID_USER_BASE | 0x1004)
@@ -487,12 +487,11 @@ static int mt9t001_s_ctrl(struct v4l2_ctrl *ctrl)
                                      ctrl->val >> 16);
 
         case V4L2_CID_TEST_PATTERN:
-                ret = mt9t001_set_output_control(mt9t001,
+                return mt9t001_set_output_control(mt9t001,
                         ctrl->val ? 0 : MT9T001_OUTPUT_CONTROL_TEST_DATA,
                         ctrl->val ? MT9T001_OUTPUT_CONTROL_TEST_DATA : 0);
-                if (ret < 0)
-                        return ret;
 
+        case V4L2_CID_TEST_PATTERN_COLOR:
                 return mt9t001_write(client, MT9T001_TEST_DATA, ctrl->val << 2);
 
         case V4L2_CID_BLACK_LEVEL_AUTO:
@@ -533,12 +532,17 @@ static struct v4l2_ctrl_ops mt9t001_ctrl_ops = {
         .s_ctrl = mt9t001_s_ctrl,
 };
 
+static const char * const mt9t001_test_pattern_menu[] = {
+        "Disabled",
+        "Enabled",
+};
+
 static const struct v4l2_ctrl_config mt9t001_ctrls[] = {
         {
                 .ops            = &mt9t001_ctrl_ops,
-                .id             = V4L2_CID_TEST_PATTERN,
+                .id             = V4L2_CID_TEST_PATTERN_COLOR,
                 .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Test pattern",
+                .name           = "Test Pattern Color",
                 .min            = 0,
                 .max            = 1023,
                 .step           = 1,
@@ -741,7 +745,7 @@ static int mt9t001_probe(struct i2c_client *client,
                 return -ENOMEM;
 
         v4l2_ctrl_handler_init(&mt9t001->ctrls, ARRAY_SIZE(mt9t001_ctrls) +
-                                                ARRAY_SIZE(mt9t001_gains) + 3);
+                                                ARRAY_SIZE(mt9t001_gains) + 4);
 
         v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
                           V4L2_CID_EXPOSURE, MT9T001_SHUTTER_WIDTH_MIN,
@@ -752,6 +756,10 @@ static int mt9t001_probe(struct i2c_client *client,
         v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
                           V4L2_CID_PIXEL_RATE, pdata->ext_clk, pdata->ext_clk,
                           1, pdata->ext_clk);
+        v4l2_ctrl_new_std_menu_items(&mt9t001->ctrls, &mt9t001_ctrl_ops,
+                        V4L2_CID_TEST_PATTERN,
+                        ARRAY_SIZE(mt9t001_test_pattern_menu) - 1, 0,
+                        0, mt9t001_test_pattern_menu);
 
         for (i = 0; i < ARRAY_SIZE(mt9t001_ctrls); ++i)
                 v4l2_ctrl_new_custom(&mt9t001->ctrls, &mt9t001_ctrls[i], NULL);
diff --git a/drivers/media/i2c/mt9v032.c b/drivers/media/i2c/mt9v032.c
index e2177405dad2..3f356cb28256 100644
--- a/drivers/media/i2c/mt9v032.c
+++ b/drivers/media/i2c/mt9v032.c
@@ -141,6 +141,10 @@ struct mt9v032 {
         u16 chip_control;
         u16 aec_agc;
         u16 hblank;
+        struct {
+                struct v4l2_ctrl *test_pattern;
+                struct v4l2_ctrl *test_pattern_color;
+        };
 };
 
 static struct mt9v032 *to_mt9v032(struct v4l2_subdev *sd)
@@ -500,7 +504,7 @@ static int mt9v032_set_crop(struct v4l2_subdev *subdev,
  * V4L2 subdev control operations
  */
 
-#define V4L2_CID_TEST_PATTERN           (V4L2_CID_USER_BASE | 0x1001)
+#define V4L2_CID_TEST_PATTERN_COLOR     (V4L2_CID_USER_BASE | 0x1001)
 
 static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
 {
@@ -545,7 +549,7 @@ static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
                 break;
 
         case V4L2_CID_TEST_PATTERN:
-                switch (ctrl->val) {
+                switch (mt9v032->test_pattern->val) {
                 case 0:
                         data = 0;
                         break;
@@ -562,13 +566,13 @@ static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
                              | MT9V032_TEST_PATTERN_ENABLE;
                         break;
                 default:
-                        data = (ctrl->val << MT9V032_TEST_PATTERN_DATA_SHIFT)
+                        data = (mt9v032->test_pattern_color->val <<
+                                MT9V032_TEST_PATTERN_DATA_SHIFT)
                              | MT9V032_TEST_PATTERN_USE_DATA
                              | MT9V032_TEST_PATTERN_ENABLE
                              | MT9V032_TEST_PATTERN_FLIP;
                         break;
                 }
-
                 return mt9v032_write(client, MT9V032_TEST_PATTERN, data);
         }
 
@@ -579,18 +583,24 @@ static struct v4l2_ctrl_ops mt9v032_ctrl_ops = {
         .s_ctrl = mt9v032_s_ctrl,
 };
 
-static const struct v4l2_ctrl_config mt9v032_ctrls[] = {
-        {
-                .ops            = &mt9v032_ctrl_ops,
-                .id             = V4L2_CID_TEST_PATTERN,
-                .type           = V4L2_CTRL_TYPE_INTEGER,
-                .name           = "Test pattern",
-                .min            = 0,
-                .max            = 1023,
-                .step           = 1,
-                .def            = 0,
-                .flags          = 0,
-        }
+static const char * const mt9v032_test_pattern_menu[] = {
+        "Disabled",
+        "Gray Vertical Shade",
+        "Gray Horizontal Shade",
+        "Gray Diagonal Shade",
+        "Plain",
+};
+
+static const struct v4l2_ctrl_config mt9v032_test_pattern_color = {
+        .ops            = &mt9v032_ctrl_ops,
+        .id             = V4L2_CID_TEST_PATTERN_COLOR,
+        .type           = V4L2_CTRL_TYPE_INTEGER,
+        .name           = "Test Pattern Color",
+        .min            = 0,
+        .max            = 1023,
+        .step           = 1,
+        .def            = 0,
+        .flags          = 0,
 };
 
 /* -----------------------------------------------------------------------------
@@ -741,7 +751,7 @@ static int mt9v032_probe(struct i2c_client *client,
         mutex_init(&mt9v032->power_lock);
         mt9v032->pdata = pdata;
 
-        v4l2_ctrl_handler_init(&mt9v032->ctrls, ARRAY_SIZE(mt9v032_ctrls) + 8);
+        v4l2_ctrl_handler_init(&mt9v032->ctrls, 10);
 
         v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
                           V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
@@ -763,6 +773,14 @@ static int mt9v032_probe(struct i2c_client *client,
                           V4L2_CID_VBLANK, MT9V032_VERTICAL_BLANKING_MIN,
                           MT9V032_VERTICAL_BLANKING_MAX, 1,
                           MT9V032_VERTICAL_BLANKING_DEF);
+        mt9v032->test_pattern = v4l2_ctrl_new_std_menu_items(&mt9v032->ctrls,
+                                &mt9v032_ctrl_ops, V4L2_CID_TEST_PATTERN,
+                                ARRAY_SIZE(mt9v032_test_pattern_menu) - 1, 0, 0,
+                                mt9v032_test_pattern_menu);
+        mt9v032->test_pattern_color = v4l2_ctrl_new_custom(&mt9v032->ctrls,
+                                      &mt9v032_test_pattern_color, NULL);
+
+        v4l2_ctrl_cluster(2, &mt9v032->test_pattern);
 
         mt9v032->pixel_rate =
                 v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
@@ -784,8 +802,6 @@ static int mt9v032_probe(struct i2c_client *client,
                 v4l2_ctrl_cluster(2, &mt9v032->link_freq);
         }
 
-        for (i = 0; i < ARRAY_SIZE(mt9v032_ctrls); ++i)
-                v4l2_ctrl_new_custom(&mt9v032->ctrls, &mt9v032_ctrls[i], NULL);
 
         mt9v032->subdev.ctrl_handler = &mt9v032->ctrls;
 
diff --git a/drivers/media/i2c/soc_camera/ov2640.c b/drivers/media/i2c/soc_camera/ov2640.c
index 78ac5744cb5d..d2d298b6354e 100644
--- a/drivers/media/i2c/soc_camera/ov2640.c
+++ b/drivers/media/i2c/soc_camera/ov2640.c
@@ -684,6 +684,11 @@ static int ov2640_s_ctrl(struct v4l2_ctrl *ctrl)
                 &container_of(ctrl->handler, struct ov2640_priv, hdl)->subdev;
         struct i2c_client  *client = v4l2_get_subdevdata(sd);
         u8 val;
+        int ret;
+
+        ret = i2c_smbus_write_byte_data(client, BANK_SEL, BANK_SEL_SENS);
+        if (ret < 0)
+                return ret;
 
         switch (ctrl->id) {
         case V4L2_CID_VFLIP:
diff --git a/drivers/media/i2c/ths7303.c b/drivers/media/i2c/ths7303.c
index e5c0eedebc58..c31cc04fffd2 100644
--- a/drivers/media/i2c/ths7303.c
+++ b/drivers/media/i2c/ths7303.c
@@ -28,6 +28,18 @@
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-chip-ident.h>
 
+#define THS7303_CHANNEL_1       1
+#define THS7303_CHANNEL_2       2
+#define THS7303_CHANNEL_3       3
+
+enum ths7303_filter_mode {
+        THS7303_FILTER_MODE_480I_576I,
+        THS7303_FILTER_MODE_480P_576P,
+        THS7303_FILTER_MODE_720P_1080I,
+        THS7303_FILTER_MODE_1080P,
+        THS7303_FILTER_MODE_DISABLE
+};
+
 MODULE_DESCRIPTION("TI THS7303 video amplifier driver");
 MODULE_AUTHOR("Chaithrika U S");
 MODULE_LICENSE("GPL");
@@ -37,35 +49,96 @@ module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "Debug level 0-1");
 
 /* following function is used to set ths7303 */
-static int ths7303_setvalue(struct v4l2_subdev *sd, v4l2_std_id std)
+int ths7303_setval(struct v4l2_subdev *sd, enum ths7303_filter_mode mode)
 {
+        u8 input_bias_chroma = 3;
+        u8 input_bias_luma = 3;
+        int disable = 0;
         int err = 0;
-        u8 val;
-        struct i2c_client *client;
+        u8 val = 0;
+        u8 temp;
 
-        client = v4l2_get_subdevdata(sd);
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
 
-        if (std & (V4L2_STD_ALL & ~V4L2_STD_SECAM)) {
-                val = 0x02;
-                v4l2_dbg(1, debug, sd, "setting value for SDTV format\n");
-        } else {
-                val = 0x00;
-                v4l2_dbg(1, debug, sd, "disabling all channels\n");
+        if (!client)
+                return -EINVAL;
+
+        switch (mode) {
+        case THS7303_FILTER_MODE_1080P:
+                val = (3 << 6);
+                val |= (3 << 3);
+                break;
+        case THS7303_FILTER_MODE_720P_1080I:
+                val = (2 << 6);
+                val |= (2 << 3);
+                break;
+        case THS7303_FILTER_MODE_480P_576P:
+                val = (1 << 6);
+                val |= (1 << 3);
+                break;
+        case THS7303_FILTER_MODE_480I_576I:
+                break;
+        case THS7303_FILTER_MODE_DISABLE:
+                pr_info("mode disabled\n");
+                /* disable all channels */
+                disable = 1;
+        default:
+                /* disable all channels */
+                disable = 1;
         }
+        /* Setup channel 2 - Luma - Green */
+        temp = val;
+        if (!disable)
+                val |= input_bias_luma;
+        err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_2, val);
+        if (err)
+                goto out;
 
-        err |= i2c_smbus_write_byte_data(client, 0x01, val);
-        err |= i2c_smbus_write_byte_data(client, 0x02, val);
-        err |= i2c_smbus_write_byte_data(client, 0x03, val);
+        /* setup two chroma channels */
+        if (!disable)
+                temp |= input_bias_chroma;
 
+        err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_1, temp);
         if (err)
-                v4l2_err(sd, "write failed\n");
+                goto out;
 
+        err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_3, temp);
+        if (err)
+                goto out;
+        return err;
+out:
+        pr_info("write byte data failed\n");
         return err;
 }
 
 static int ths7303_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
 {
-        return ths7303_setvalue(sd, norm);
+        if (norm & (V4L2_STD_ALL & ~V4L2_STD_SECAM))
+                return ths7303_setval(sd, THS7303_FILTER_MODE_480I_576I);
+        else
+                return ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
+}
+
+/* for setting filter for HD output */
+static int ths7303_s_dv_timings(struct v4l2_subdev *sd,
+                               struct v4l2_dv_timings *dv_timings)
+{
+        u32 height = dv_timings->bt.height;
+        int interlaced = dv_timings->bt.interlaced;
+        int res = 0;
+
+        if (height == 1080 && !interlaced)
+                res = ths7303_setval(sd, THS7303_FILTER_MODE_1080P);
+        else if ((height == 720 && !interlaced) ||
+                        (height == 1080 && interlaced))
+                res = ths7303_setval(sd, THS7303_FILTER_MODE_720P_1080I);
+        else if ((height == 480 || height == 576) && !interlaced)
+                res = ths7303_setval(sd, THS7303_FILTER_MODE_480P_576P);
+        else
+                /* disable all channels */
+                res = ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
+
+        return res;
 }
 
 static int ths7303_g_chip_ident(struct v4l2_subdev *sd,
@@ -78,6 +151,7 @@ static int ths7303_g_chip_ident(struct v4l2_subdev *sd,
 
 static const struct v4l2_subdev_video_ops ths7303_video_ops = {
         .s_std_output   = ths7303_s_std_output,
+        .s_dv_timings    = ths7303_s_dv_timings,
 };
 
 static const struct v4l2_subdev_core_ops ths7303_core_ops = {
@@ -107,7 +181,7 @@ static int ths7303_probe(struct i2c_client *client,
 
         v4l2_i2c_subdev_init(sd, client, &ths7303_ops);
 
-        return ths7303_setvalue(sd, std_id);
+        return ths7303_s_std_output(sd, std_id);
 }
 
 static int ths7303_remove(struct i2c_client *client)
diff --git a/drivers/media/i2c/tvp514x.c b/drivers/media/i2c/tvp514x.c
index 1f3943bb87d5..d5e10215a28f 100644
--- a/drivers/media/i2c/tvp514x.c
+++ b/drivers/media/i2c/tvp514x.c
@@ -519,6 +519,12 @@ static int tvp514x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std_id)
 
         *std_id = V4L2_STD_UNKNOWN;
 
+        /* To query the standard the TVP514x must power on the ADCs. */
+        if (!decoder->streaming) {
+                tvp514x_s_stream(sd, 1);
+                msleep(LOCK_RETRY_DELAY);
+        }
+
         /* query the current standard */
         current_std = tvp514x_query_current_std(sd);
         if (current_std == STD_INVALID)
@@ -625,25 +631,12 @@ static int tvp514x_s_routing(struct v4l2_subdev *sd,
         int err;
         enum tvp514x_input input_sel;
         enum tvp514x_output output_sel;
-        u8 sync_lock_status, lock_mask;
-        int try_count = LOCK_RETRY_COUNT;
 
         if ((input >= INPUT_INVALID) ||
                         (output >= OUTPUT_INVALID))
                 /* Index out of bound */
                 return -EINVAL;
 
-        /*
-         * For the sequence streamon -> streamoff and again s_input
-         * it fails to lock the signal, since streamoff puts TVP514x
-         * into power off state which leads to failure in sub-sequent s_input.
-         *
-         * So power up the TVP514x device here, since it is important to lock
-         * the signal at this stage.
-         */
-        if (!decoder->streaming)
-                tvp514x_s_stream(sd, 1);
-
         input_sel = input;
         output_sel = output;
 
@@ -660,64 +653,6 @@ static int tvp514x_s_routing(struct v4l2_subdev *sd,
 
         decoder->tvp514x_regs[REG_INPUT_SEL].val = input_sel;
         decoder->tvp514x_regs[REG_OUTPUT_FORMATTER1].val = output_sel;
-
-        /* Clear status */
-        msleep(LOCK_RETRY_DELAY);
-        err =
-            tvp514x_write_reg(sd, REG_CLEAR_LOST_LOCK, 0x01);
-        if (err)
-                return err;
-
-        switch (input_sel) {
-        case INPUT_CVBS_VI1A:
-        case INPUT_CVBS_VI1B:
-        case INPUT_CVBS_VI1C:
-        case INPUT_CVBS_VI2A:
-        case INPUT_CVBS_VI2B:
-        case INPUT_CVBS_VI2C:
-        case INPUT_CVBS_VI3A:
-        case INPUT_CVBS_VI3B:
-        case INPUT_CVBS_VI3C:
-        case INPUT_CVBS_VI4A:
-                lock_mask = STATUS_CLR_SUBCAR_LOCK_BIT |
-                        STATUS_HORZ_SYNC_LOCK_BIT |
-                        STATUS_VIRT_SYNC_LOCK_BIT;
-                break;
-
-        case INPUT_SVIDEO_VI2A_VI1A:
-        case INPUT_SVIDEO_VI2B_VI1B:
-        case INPUT_SVIDEO_VI2C_VI1C:
-        case INPUT_SVIDEO_VI2A_VI3A:
-        case INPUT_SVIDEO_VI2B_VI3B:
-        case INPUT_SVIDEO_VI2C_VI3C:
-        case INPUT_SVIDEO_VI4A_VI1A:
-        case INPUT_SVIDEO_VI4A_VI1B:
-        case INPUT_SVIDEO_VI4A_VI1C:
-        case INPUT_SVIDEO_VI4A_VI3A:
-        case INPUT_SVIDEO_VI4A_VI3B:
-        case INPUT_SVIDEO_VI4A_VI3C:
-                lock_mask = STATUS_HORZ_SYNC_LOCK_BIT |
-                        STATUS_VIRT_SYNC_LOCK_BIT;
-                break;
-        /* Need to add other interfaces*/
-        default:
-                return -EINVAL;
-        }
-
-        while (try_count-- > 0) {
-                /* Allow decoder to sync up with new input */
-                msleep(LOCK_RETRY_DELAY);
-
-                sync_lock_status = tvp514x_read_reg(sd,
-                                REG_STATUS1);
-                if (lock_mask == (sync_lock_status & lock_mask))
-                        /* Input detected */
-                        break;
-        }
-
-        if (try_count < 0)
-                return -EINVAL;
-
         decoder->input = input;
         decoder->output = output;
 
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index b68918c97f66..56c6c77793d7 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -668,6 +668,12 @@ static const struct v4l2_queryctrl bttv_ctls[] = {
                 .default_value = 32768,
                 .type          = V4L2_CTRL_TYPE_INTEGER,
         },{
+                .id            = V4L2_CID_COLOR_KILLER,
+                .name          = "Color killer",
+                .minimum       = 0,
+                .maximum       = 1,
+                .type          = V4L2_CTRL_TYPE_BOOLEAN,
+        }, {
                 .id            = V4L2_CID_HUE,
                 .name          = "Hue",
                 .minimum       = 0,
@@ -1474,6 +1480,9 @@ static int bttv_g_ctrl(struct file *file, void *priv,
         case V4L2_CID_SATURATION:
                 c->value = btv->saturation;
                 break;
+        case V4L2_CID_COLOR_KILLER:
+                c->value = btv->opt_color_killer;
+                break;
 
         case V4L2_CID_AUDIO_MUTE:
         case V4L2_CID_AUDIO_VOLUME:
@@ -1526,7 +1535,6 @@ static int bttv_s_ctrl(struct file *file, void *f,
                                         struct v4l2_control *c)
 {
         int err;
-        int val;
         struct bttv_fh *fh = f;
         struct bttv *btv = fh->btv;
 
@@ -1547,6 +1555,16 @@ static int bttv_s_ctrl(struct file *file, void *f,
         case V4L2_CID_SATURATION:
                 bt848_sat(btv, c->value);
                 break;
+        case V4L2_CID_COLOR_KILLER:
+                btv->opt_color_killer = c->value;
+                if (btv->opt_color_killer) {
+                        btor(BT848_SCLOOP_CKILL, BT848_E_SCLOOP);
+                        btor(BT848_SCLOOP_CKILL, BT848_O_SCLOOP);
+                } else {
+                        btand(~BT848_SCLOOP_CKILL, BT848_E_SCLOOP);
+                        btand(~BT848_SCLOOP_CKILL, BT848_O_SCLOOP);
+                }
+                break;
         case V4L2_CID_AUDIO_MUTE:
                 audio_mute(btv, c->value);
                 /* fall through */
@@ -1564,9 +1582,13 @@ static int bttv_s_ctrl(struct file *file, void *f,
 
         case V4L2_CID_PRIVATE_CHROMA_AGC:
                 btv->opt_chroma_agc = c->value;
-                val = btv->opt_chroma_agc ? BT848_SCLOOP_CAGC : 0;
-                btwrite(val, BT848_E_SCLOOP);
-                btwrite(val, BT848_O_SCLOOP);
+                if (btv->opt_chroma_agc) {
+                        btor(BT848_SCLOOP_CAGC, BT848_E_SCLOOP);
+                        btor(BT848_SCLOOP_CAGC, BT848_O_SCLOOP);
+                } else {
+                        btand(~BT848_SCLOOP_CAGC, BT848_E_SCLOOP);
+                        btand(~BT848_SCLOOP_CAGC, BT848_O_SCLOOP);
+                }
                 break;
         case V4L2_CID_PRIVATE_COMBFILTER:
                 btv->opt_combfilter = c->value;
diff --git a/drivers/media/pci/bt8xx/bttvp.h b/drivers/media/pci/bt8xx/bttvp.h
index 70fd4f23f605..9ec0adba236c 100644
--- a/drivers/media/pci/bt8xx/bttvp.h
+++ b/drivers/media/pci/bt8xx/bttvp.h
@@ -429,6 +429,7 @@ struct bttv {
         int opt_lumafilter;
         int opt_automute;
         int opt_chroma_agc;
+        int opt_color_killer;
         int opt_adc_crush;
         int opt_vcr_hack;
         int opt_whitecrush_upper;
diff --git a/drivers/media/pci/bt8xx/dst_ca.c b/drivers/media/pci/bt8xx/dst_ca.c
index ee3884fbc9ce..7d96fab7d246 100644
--- a/drivers/media/pci/bt8xx/dst_ca.c
+++ b/drivers/media/pci/bt8xx/dst_ca.c
@@ -646,7 +646,7 @@ static long dst_ca_ioctl(struct file *file, unsigned int cmd, unsigned long ioct
                 dprintk(verbose, DST_CA_INFO, 1, " -->CA_SET_PID Success !");
         default:
                 result = -EOPNOTSUPP;
-        };
+        }
  free_mem_and_exit:
         kfree (p_ca_message);
         kfree (p_ca_slot_info);
diff --git a/drivers/media/pci/cx23885/altera-ci.c b/drivers/media/pci/cx23885/altera-ci.c
index aee7f0dacff1..495781ee4711 100644
--- a/drivers/media/pci/cx23885/altera-ci.c
+++ b/drivers/media/pci/cx23885/altera-ci.c
@@ -416,7 +416,7 @@ static void netup_read_ci_status(struct work_struct *work)
                                 DVB_CA_EN50221_POLL_CAM_READY : 0);
                 ci_dbg_print("%s: setting CI[1] status = 0x%x\n",
                                 __func__, inter->state[1]->status);
-        };
+        }
 
         if (inter->state[0] != NULL) {
                 inter->state[0]->status =
@@ -425,7 +425,7 @@ static void netup_read_ci_status(struct work_struct *work)
                                 DVB_CA_EN50221_POLL_CAM_READY : 0);
                 ci_dbg_print("%s: setting CI[0] status = 0x%x\n",
                                 __func__, inter->state[0]->status);
-        };
+        }
 }
 
 /* CI irq handler */
diff --git a/drivers/media/pci/cx23885/cimax2.c b/drivers/media/pci/cx23885/cimax2.c
index c9f15d6dec40..6617774a326a 100644
--- a/drivers/media/pci/cx23885/cimax2.c
+++ b/drivers/media/pci/cx23885/cimax2.c
@@ -193,7 +193,7 @@ int netup_ci_op_cam(struct dvb_ca_en50221 *en50221, int slot,
                                 0, &store, 1);
                 if (ret != 0)
                         return ret;
-        };
+        }
         state->current_ci_flag = flag;
 
         mutex_lock(&dev->gpio_lock);
diff --git a/drivers/media/pci/cx23885/cx23885-cards.c b/drivers/media/pci/cx23885/cx23885-cards.c
index 39a4a4b9ed7e..5acdf954ff6b 100644
--- a/drivers/media/pci/cx23885/cx23885-cards.c
+++ b/drivers/media/pci/cx23885/cx23885-cards.c
@@ -542,11 +542,13 @@ struct cx23885_board cx23885_boards[] = {
                         {
                                 .type   = CX23885_VMUX_COMPOSITE1,
                                 .vmux   = CX25840_COMPOSITE8,
+                                .amux   = CX25840_AUDIO7,
                         },
                         {
                                 .type   = CX23885_VMUX_SVIDEO,
                                 .vmux   = CX25840_SVIDEO_LUMA3 |
                                                 CX25840_SVIDEO_CHROMA4,
+                                .amux   = CX25840_AUDIO7,
                         },
                         {
                                 .type   = CX23885_VMUX_COMPONENT,
@@ -554,6 +556,7 @@ struct cx23885_board cx23885_boards[] = {
                                         CX25840_VIN1_CH1 |
                                         CX25840_VIN6_CH2 |
                                         CX25840_VIN7_CH3,
+                                .amux   = CX25840_AUDIO7,
                         },
                 },
         },
diff --git a/drivers/media/pci/cx23885/cx23885-video.c b/drivers/media/pci/cx23885/cx23885-video.c
index 8c4a9a5f9a50..1a21926ca412 100644
--- a/drivers/media/pci/cx23885/cx23885-video.c
+++ b/drivers/media/pci/cx23885/cx23885-video.c
@@ -508,7 +508,8 @@ static int cx23885_video_mux(struct cx23885_dev *dev, unsigned int input)
                 (dev->board == CX23885_BOARD_HAUPPAUGE_HVR1250) ||
                 (dev->board == CX23885_BOARD_HAUPPAUGE_HVR1255) ||
                 (dev->board == CX23885_BOARD_HAUPPAUGE_HVR1255_22111) ||
-                (dev->board == CX23885_BOARD_HAUPPAUGE_HVR1850)) {
+                (dev->board == CX23885_BOARD_HAUPPAUGE_HVR1850) ||
+                (dev->board == CX23885_BOARD_MYGICA_X8507)) {
                 /* Configure audio routing */
                 v4l2_subdev_call(dev->sd_cx25840, audio, s_routing,
                         INPUT(input)->amux, 0, 0);
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c b/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
index c8c94fbf5d8d..d33fc1a23030 100644
--- a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
+++ b/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
@@ -761,7 +761,7 @@ int cx25821_vidupstream_init_ch2(struct cx25821_dev *dev, int channel_select,
         }
 
         /* Default if filename is empty string */
-        if (strcmp(dev->input_filename_ch2, "") == 0) {
+        if (strcmp(dev->_filename_ch2, "") == 0) {
                 if (dev->_isNTSC_ch2) {
                         dev->_filename_ch2 = (dev->_pixel_format_ch2 ==
                                 PIXEL_FRMT_411) ? "/root/vid411.yuv" :
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream.c b/drivers/media/pci/cx25821/cx25821-video-upstream.c
index 52c13e0b6492..6759fff8eb64 100644
--- a/drivers/media/pci/cx25821/cx25821-video-upstream.c
+++ b/drivers/media/pci/cx25821/cx25821-video-upstream.c
@@ -808,7 +808,7 @@ int cx25821_vidupstream_init_ch1(struct cx25821_dev *dev, int channel_select,
         }
 
         /* Default if filename is empty string */
-        if (strcmp(dev->input_filename, "") == 0) {
+        if (strcmp(dev->_filename, "") == 0) {
                 if (dev->_isNTSC) {
                         dev->_filename =
                                 (dev->_pixel_format == PIXEL_FRMT_411) ?
diff --git a/drivers/media/pci/cx88/cx88-blackbird.c b/drivers/media/pci/cx88/cx88-blackbird.c
index def363fb71c0..62184eb919e5 100644
--- a/drivers/media/pci/cx88/cx88-blackbird.c
+++ b/drivers/media/pci/cx88/cx88-blackbird.c
@@ -721,7 +721,7 @@ static int vidioc_g_fmt_vid_cap (struct file *file, void *priv,
 
         f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
         f->fmt.pix.bytesperline = 0;
-        f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */;
+        f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */
         f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
         f->fmt.pix.width        = dev->width;
         f->fmt.pix.height       = dev->height;
@@ -739,7 +739,7 @@ static int vidioc_try_fmt_vid_cap (struct file *file, void *priv,
 
         f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
         f->fmt.pix.bytesperline = 0;
-        f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */;
+        f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */
         f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
         dprintk(1, "VIDIOC_TRY_FMT: w: %d, h: %d, f: %d\n",
                 dev->width, dev->height, fh->mpegq.field );
@@ -755,7 +755,7 @@ static int vidioc_s_fmt_vid_cap (struct file *file, void *priv,
 
         f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
         f->fmt.pix.bytesperline = 0;
-        f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */;
+        f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */
         f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
         dev->width              = f->fmt.pix.width;
         dev->height             = f->fmt.pix.height;
diff --git a/drivers/media/pci/cx88/cx88-dvb.c b/drivers/media/pci/cx88/cx88-dvb.c
index d803bba09525..666f83b2f3c0 100644
--- a/drivers/media/pci/cx88/cx88-dvb.c
+++ b/drivers/media/pci/cx88/cx88-dvb.c
@@ -896,7 +896,7 @@ static int samsung_smt_7020_set_voltage(struct dvb_frontend *fe,
                 break;
         default:
                 return -EINVAL;
-        };
+        }
 
         return (i2c_transfer(&dev->core->i2c_adap, &msg, 1) == 1) ? 0 : -EIO;
 }
diff --git a/drivers/media/pci/cx88/cx88-mpeg.c b/drivers/media/pci/cx88/cx88-mpeg.c
index c04fb618e10b..d154bc197356 100644
--- a/drivers/media/pci/cx88/cx88-mpeg.c
+++ b/drivers/media/pci/cx88/cx88-mpeg.c
@@ -450,7 +450,7 @@ static irqreturn_t cx8802_irq(int irq, void *dev_id)
                         cx88_core_irq(core,status);
                 if (status & PCI_INT_TSINT)
                         cx8802_mpeg_irq(dev);
-        };
+        }
         if (MAX_IRQ_LOOP == loop) {
                 dprintk( 0, "clearing mask\n" );
                 printk(KERN_WARNING "%s/0: irq loop -- clearing mask\n",
diff --git a/drivers/media/pci/cx88/cx88-tvaudio.c b/drivers/media/pci/cx88/cx88-tvaudio.c
index 770ec05b5e9b..424fd97495dc 100644
--- a/drivers/media/pci/cx88/cx88-tvaudio.c
+++ b/drivers/media/pci/cx88/cx88-tvaudio.c
@@ -373,7 +373,7 @@ static void set_audio_standard_NICAM(struct cx88_core *core, u32 mode)
                 set_audio_registers(core, nicam_bgdki_common);
                 set_audio_registers(core, nicam_default);
                 break;
-        };
+        }
 
         mode |= EN_DMTRX_LR | EN_DMTRX_BYPASS;
         set_audio_finish(core, mode);
@@ -639,7 +639,7 @@ static void set_audio_standard_A2(struct cx88_core *core, u32 mode)
                 dprintk("%s Warning: wrong value\n", __func__);
                 return;
                 break;
-        };
+        }
 
         mode |= EN_FMRADIO_EN_RDS | EN_DMTRX_SUMDIFF;
         set_audio_finish(core, mode);
diff --git a/drivers/media/pci/cx88/cx88-video.c b/drivers/media/pci/cx88/cx88-video.c
index a146d50d7795..05171457bf28 100644
--- a/drivers/media/pci/cx88/cx88-video.c
+++ b/drivers/media/pci/cx88/cx88-video.c
@@ -1535,7 +1535,7 @@ static irqreturn_t cx8800_irq(int irq, void *dev_id)
                         cx88_core_irq(core,status);
                 if (status & PCI_INT_VIDINT)
                         cx8800_vid_irq(dev);
-        };
+        }
         if (10 == loop) {
                 printk(KERN_WARNING "%s/0: irq loop -- clearing mask\n",
                        core->name);
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index 22f8758d047f..4a77124ee70e 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -1204,7 +1204,7 @@ int saa7134_s_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, str
                 break;
         default:
                 /* nothing */;
-        };
+        }
         switch (c->id) {
         case V4L2_CID_BRIGHTNESS:
                 dev->ctl_bright = c->value;
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index f588d6296c76..181c7686e412 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -165,12 +165,12 @@ config VIDEO_SAMSUNG_S5P_JPEG
           This is a v4l2 driver for Samsung S5P and EXYNOS4 JPEG codec
 
 config VIDEO_SAMSUNG_S5P_MFC
-        tristate "Samsung S5P MFC 5.1 Video Codec"
+        tristate "Samsung S5P MFC Video Codec"
         depends on VIDEO_DEV && VIDEO_V4L2 && PLAT_S5P
         select VIDEOBUF2_DMA_CONTIG
         default n
         help
-            MFC 5.1 driver for V4L2.
+            MFC 5.1 and 6.x driver for V4L2
 
 config VIDEO_MX2_EMMAPRP
         tristate "MX2 eMMa-PrP support"
diff --git a/drivers/media/platform/davinci/vpbe.c b/drivers/media/platform/davinci/vpbe.c
index c4a82a1a8a97..69d7a58c92c3 100644
--- a/drivers/media/platform/davinci/vpbe.c
+++ b/drivers/media/platform/davinci/vpbe.c
@@ -174,26 +174,6 @@ static int vpbe_get_current_mode_info(struct vpbe_device *vpbe_dev,
         return 0;
 }
 
-static int vpbe_get_dv_preset_info(struct vpbe_device *vpbe_dev,
-                                   unsigned int dv_preset)
-{
-        struct vpbe_config *cfg = vpbe_dev->cfg;
-        struct vpbe_enc_mode_info var;
-        int curr_output = vpbe_dev->current_out_index;
-        int i;
-
-        for (i = 0; i < vpbe_dev->cfg->outputs[curr_output].num_modes; i++) {
-                var = cfg->outputs[curr_output].modes[i];
-                if ((var.timings_type & VPBE_ENC_DV_PRESET) &&
-                  (var.timings.dv_preset == dv_preset)) {
-                        vpbe_dev->current_timings = var;
-                        return 0;
-                }
-        }
-
-        return -EINVAL;
-}
-
 /* Get std by std id */
 static int vpbe_get_std_info(struct vpbe_device *vpbe_dev,
                              v4l2_std_id std_id)
@@ -206,7 +186,7 @@ static int vpbe_get_std_info(struct vpbe_device *vpbe_dev,
         for (i = 0; i < vpbe_dev->cfg->outputs[curr_output].num_modes; i++) {
                 var = cfg->outputs[curr_output].modes[i];
                 if ((var.timings_type & VPBE_ENC_STD) &&
-                  (var.timings.std_id & std_id)) {
+                  (var.std_id & std_id)) {
                         vpbe_dev->current_timings = var;
                         return 0;
                 }
@@ -344,38 +324,42 @@ static unsigned int vpbe_get_output(struct vpbe_device *vpbe_dev)
 }
 
 /**
- * vpbe_s_dv_preset - Set the given preset timings in the encoder
+ * vpbe_s_dv_timings - Set the given preset timings in the encoder
  *
- * Sets the preset if supported by the current encoder. Return the status.
+ * Sets the timings if supported by the current encoder. Return the status.
  * 0 - success & -EINVAL on error
  */
-static int vpbe_s_dv_preset(struct vpbe_device *vpbe_dev,
-                     struct v4l2_dv_preset *dv_preset)
+static int vpbe_s_dv_timings(struct vpbe_device *vpbe_dev,
+                    struct v4l2_dv_timings *dv_timings)
 {
         struct vpbe_config *cfg = vpbe_dev->cfg;
         int out_index = vpbe_dev->current_out_index;
+        struct vpbe_output *output = &cfg->outputs[out_index];
         int sd_index = vpbe_dev->current_sd_index;
-        int ret;
+        int ret, i;
 
 
         if (!(cfg->outputs[out_index].output.capabilities &
-            V4L2_OUT_CAP_PRESETS))
+            V4L2_OUT_CAP_DV_TIMINGS))
                 return -EINVAL;
 
-        ret = vpbe_get_dv_preset_info(vpbe_dev, dv_preset->preset);
-
-        if (ret)
-                return ret;
-
+        for (i = 0; i < output->num_modes; i++) {
+                if (output->modes[i].timings_type == VPBE_ENC_CUSTOM_TIMINGS &&
+                    !memcmp(&output->modes[i].dv_timings,
+                                dv_timings, sizeof(*dv_timings)))
+                        break;
+        }
+        if (i >= output->num_modes)
+                return -EINVAL;
+        vpbe_dev->current_timings = output->modes[i];
         mutex_lock(&vpbe_dev->lock);
 
-
         ret = v4l2_subdev_call(vpbe_dev->encoders[sd_index], video,
-                                        s_dv_preset, dv_preset);
+                                        s_dv_timings, dv_timings);
         if (!ret && (vpbe_dev->amp != NULL)) {
                 /* Call amplifier subdevice */
                 ret = v4l2_subdev_call(vpbe_dev->amp, video,
-                                s_dv_preset, dv_preset);
+                                s_dv_timings, dv_timings);
         }
         /* set the lcd controller output for the given mode */
         if (!ret) {
@@ -392,17 +376,17 @@ static int vpbe_s_dv_preset(struct vpbe_device *vpbe_dev,
 }
 
 /**
- * vpbe_g_dv_preset - Get the preset in the current encoder
+ * vpbe_g_dv_timings - Get the timings in the current encoder
  *
- * Get the preset in the current encoder. Return the status. 0 - success
+ * Get the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
-static int vpbe_g_dv_preset(struct vpbe_device *vpbe_dev,
-                     struct v4l2_dv_preset *dv_preset)
+static int vpbe_g_dv_timings(struct vpbe_device *vpbe_dev,
+                     struct v4l2_dv_timings *dv_timings)
 {
         if (vpbe_dev->current_timings.timings_type &
-          VPBE_ENC_DV_PRESET) {
-                dv_preset->preset = vpbe_dev->current_timings.timings.dv_preset;
+          VPBE_ENC_CUSTOM_TIMINGS) {
+                *dv_timings = vpbe_dev->current_timings.dv_timings;
                 return 0;
         }
 
@@ -410,13 +394,13 @@ static int vpbe_g_dv_preset(struct vpbe_device *vpbe_dev,
 }
 
 /**
- * vpbe_enum_dv_presets - Enumerate the dv presets in the current encoder
+ * vpbe_enum_dv_timings - Enumerate the dv timings in the current encoder
  *
- * Get the preset in the current encoder. Return the status. 0 - success
+ * Get the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
-static int vpbe_enum_dv_presets(struct vpbe_device *vpbe_dev,
-                         struct v4l2_dv_enum_preset *preset_info)
+static int vpbe_enum_dv_timings(struct vpbe_device *vpbe_dev,
+                         struct v4l2_enum_dv_timings *timings)
 {
         struct vpbe_config *cfg = vpbe_dev->cfg;
         int out_index = vpbe_dev->current_out_index;
@@ -424,12 +408,12 @@ static int vpbe_enum_dv_presets(struct vpbe_device *vpbe_dev,
         int j = 0;
         int i;
 
-        if (!(output->output.capabilities & V4L2_OUT_CAP_PRESETS))
+        if (!(output->output.capabilities & V4L2_OUT_CAP_DV_TIMINGS))
                 return -EINVAL;
 
         for (i = 0; i < output->num_modes; i++) {
-                if (output->modes[i].timings_type == VPBE_ENC_DV_PRESET) {
-                        if (j == preset_info->index)
+                if (output->modes[i].timings_type == VPBE_ENC_CUSTOM_TIMINGS) {
+                        if (j == timings->index)
                                 break;
                         j++;
                 }
@@ -437,9 +421,8 @@ static int vpbe_enum_dv_presets(struct vpbe_device *vpbe_dev,
 
         if (i == output->num_modes)
                 return -EINVAL;
-
-        return v4l_fill_dv_preset_info(output->modes[i].timings.dv_preset,
-                                        preset_info);
+        timings->timings = output->modes[i].dv_timings;
+        return 0;
 }
 
 /**
@@ -489,10 +472,10 @@ static int vpbe_s_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
  */
 static int vpbe_g_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
 {
-        struct vpbe_enc_mode_info cur_timings = vpbe_dev->current_timings;
+        struct vpbe_enc_mode_info *cur_timings = &vpbe_dev->current_timings;
 
-        if (cur_timings.timings_type & VPBE_ENC_STD) {
-                *std_id = cur_timings.timings.std_id;
+        if (cur_timings->timings_type & VPBE_ENC_STD) {
+                *std_id = cur_timings->std_id;
                 return 0;
         }
 
@@ -511,7 +494,7 @@ static int vpbe_set_mode(struct vpbe_device *vpbe_dev,
 {
         struct vpbe_enc_mode_info *preset_mode = NULL;
         struct vpbe_config *cfg = vpbe_dev->cfg;
-        struct v4l2_dv_preset dv_preset;
+        struct v4l2_dv_timings dv_timings;
         struct osd_state *osd_device;
         int out_index = vpbe_dev->current_out_index;
         int ret = 0;
@@ -530,11 +513,12 @@ static int vpbe_set_mode(struct vpbe_device *vpbe_dev,
                          */
                         if (preset_mode->timings_type & VPBE_ENC_STD)
                                 return vpbe_s_std(vpbe_dev,
-                                                 &preset_mode->timings.std_id);
-                        if (preset_mode->timings_type & VPBE_ENC_DV_PRESET) {
-                                dv_preset.preset =
-                                        preset_mode->timings.dv_preset;
-                                return vpbe_s_dv_preset(vpbe_dev, &dv_preset);
+                                                 &preset_mode->std_id);
+                        if (preset_mode->timings_type &
+                                                VPBE_ENC_CUSTOM_TIMINGS) {
+                                dv_timings =
+                                        preset_mode->dv_timings;
+                                return vpbe_s_dv_timings(vpbe_dev, &dv_timings);
                         }
                 }
         }
@@ -626,11 +610,11 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
                 vpbe_dev->dac_clk = clk_get(vpbe_dev->pdev, "vpss_dac");
                 if (IS_ERR(vpbe_dev->dac_clk)) {
                         ret =  PTR_ERR(vpbe_dev->dac_clk);
-                        goto vpbe_unlock;
+                        goto fail_mutex_unlock;
                 }
                 if (clk_enable(vpbe_dev->dac_clk)) {
                         ret =  -ENODEV;
-                        goto vpbe_unlock;
+                        goto fail_mutex_unlock;
                 }
         }
 
@@ -642,7 +626,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
         if (ret) {
                 v4l2_err(dev->driver,
                         "Unable to register v4l2 device.\n");
-                goto vpbe_fail_clock;
+                goto fail_clk_put;
         }
         v4l2_info(&vpbe_dev->v4l2_dev, "vpbe v4l2 device registered\n");
 
@@ -658,7 +642,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
                 v4l2_err(&vpbe_dev->v4l2_dev,
                         "vpbe unable to init venc sub device\n");
                 ret = -ENODEV;
-                goto vpbe_fail_v4l2_device;
+                goto fail_dev_unregister;
         }
         /* initialize osd device */
         osd_device = vpbe_dev->osd_device;
@@ -669,7 +653,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
                         v4l2_err(&vpbe_dev->v4l2_dev,
                                  "unable to initialize the OSD device");
                         err = -ENOMEM;
-                        goto vpbe_fail_v4l2_device;
+                        goto fail_dev_unregister;
                 }
         }
 
@@ -685,7 +669,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
                 v4l2_err(&vpbe_dev->v4l2_dev,
                         "unable to allocate memory for encoders sub devices");
                 ret = -ENOMEM;
-                goto vpbe_fail_v4l2_device;
+                goto fail_dev_unregister;
         }
 
         i2c_adap = i2c_get_adapter(vpbe_dev->cfg->i2c_adapter_id);
@@ -711,7 +695,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
                                          " failed to register",
                                          enc_info->module_name);
                                 ret = -ENODEV;
-                                goto vpbe_fail_sd_register;
+                                goto fail_kfree_encoders;
                         }
                 } else
                         v4l2_warn(&vpbe_dev->v4l2_dev, "non-i2c encoders"
@@ -730,7 +714,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
                                          "amplifier %s failed to register",
                                          amp_info->module_name);
                                 ret = -ENODEV;
-                                goto vpbe_fail_amp_register;
+                                goto fail_kfree_encoders;
                         }
                         v4l2_info(&vpbe_dev->v4l2_dev,
                                           "v4l2 sub device %s registered\n",
@@ -770,16 +754,14 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
         /* TBD handling of bootargs for default output and mode */
         return 0;
 
-vpbe_fail_amp_register:
-        kfree(vpbe_dev->amp);
-vpbe_fail_sd_register:
+fail_kfree_encoders:
         kfree(vpbe_dev->encoders);
-vpbe_fail_v4l2_device:
+fail_dev_unregister:
         v4l2_device_unregister(&vpbe_dev->v4l2_dev);
-vpbe_fail_clock:
+fail_clk_put:
         if (strcmp(vpbe_dev->cfg->module_name, "dm644x-vpbe-display") != 0)
                 clk_put(vpbe_dev->dac_clk);
-vpbe_unlock:
+fail_mutex_unlock:
         mutex_unlock(&vpbe_dev->lock);
         return ret;
 }
@@ -810,9 +792,9 @@ static struct vpbe_device_ops vpbe_dev_ops = {
         .enum_outputs = vpbe_enum_outputs,
         .set_output = vpbe_set_output,
         .get_output = vpbe_get_output,
-        .s_dv_preset = vpbe_s_dv_preset,
-        .g_dv_preset = vpbe_g_dv_preset,
-        .enum_dv_presets = vpbe_enum_dv_presets,
+        .s_dv_timings = vpbe_s_dv_timings,
+        .g_dv_timings = vpbe_g_dv_timings,
+        .enum_dv_timings = vpbe_enum_dv_timings,
         .s_std = vpbe_s_std,
         .g_std = vpbe_g_std,
         .initialize = vpbe_initialize,
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
index 239f37bfa313..161c77650e2f 100644
--- a/drivers/media/platform/davinci/vpbe_display.c
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -393,7 +393,7 @@ vpbe_disp_calculate_scale_factor(struct vpbe_display *disp_dev,
         int h_scale;
         int v_scale;
 
-        v4l2_std_id standard_id = vpbe_dev->current_timings.timings.std_id;
+        v4l2_std_id standard_id = vpbe_dev->current_timings.std_id;
 
         /*
          * Application initially set the image format. Current display
@@ -637,7 +637,7 @@ static int vpbe_display_s_crop(struct file *file, void *priv,
         struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
         struct osd_layer_config *cfg = &layer->layer_info.config;
         struct osd_state *osd_device = disp_dev->osd_device;
-        struct v4l2_rect *rect = &crop->c;
+        struct v4l2_rect rect = crop->c;
         int ret;
 
         v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
@@ -648,21 +648,21 @@ static int vpbe_display_s_crop(struct file *file, void *priv,
                 return -EINVAL;
         }
 
-        if (rect->top < 0)
-                rect->top = 0;
-        if (rect->left < 0)
-                rect->left = 0;
+        if (rect.top < 0)
+                rect.top = 0;
+        if (rect.left < 0)
+                rect.left = 0;
 
-        vpbe_disp_check_window_params(disp_dev, rect);
+        vpbe_disp_check_window_params(disp_dev, &rect);
 
         osd_device->ops.get_layer_config(osd_device,
                         layer->layer_info.id, cfg);
 
         vpbe_disp_calculate_scale_factor(disp_dev, layer,
-                                        rect->width,
-                                        rect->height);
-        vpbe_disp_adj_position(disp_dev, layer, rect->top,
-                                        rect->left);
+                                        rect.width,
+                                        rect.height);
+        vpbe_disp_adj_position(disp_dev, layer, rect.top,
+                                        rect.left);
         ret = osd_device->ops.set_layer_config(osd_device,
                                 layer->layer_info.id, cfg);
         if (ret < 0) {
@@ -943,7 +943,7 @@ static int vpbe_display_g_std(struct file *file, void *priv,
 
         /* Get the standard from the current encoder */
         if (vpbe_dev->current_timings.timings_type & VPBE_ENC_STD) {
-                *std_id = vpbe_dev->current_timings.timings.std_id;
+                *std_id = vpbe_dev->current_timings.std_id;
                 return 0;
         }
 
@@ -1029,29 +1029,29 @@ static int vpbe_display_g_output(struct file *file, void *priv,
 }
 
 /**
- * vpbe_display_enum_dv_presets - Enumerate the dv presets
+ * vpbe_display_enum_dv_timings - Enumerate the dv timings
  *
- * enum the preset in the current encoder. Return the status. 0 - success
+ * enum the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
 static int
-vpbe_display_enum_dv_presets(struct file *file, void *priv,
-                        struct v4l2_dv_enum_preset *preset)
+vpbe_display_enum_dv_timings(struct file *file, void *priv,
+                        struct v4l2_enum_dv_timings *timings)
 {
         struct vpbe_fh *fh = priv;
         struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
         int ret;
 
-        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_ENUM_DV_PRESETS\n");
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_ENUM_DV_TIMINGS\n");
 
         /* Enumerate outputs */
-        if (NULL == vpbe_dev->ops.enum_dv_presets)
+        if (NULL == vpbe_dev->ops.enum_dv_timings)
                 return -EINVAL;
 
-        ret = vpbe_dev->ops.enum_dv_presets(vpbe_dev, preset);
+        ret = vpbe_dev->ops.enum_dv_timings(vpbe_dev, timings);
         if (ret) {
                 v4l2_err(&vpbe_dev->v4l2_dev,
-                        "Failed to enumerate dv presets info\n");
+                        "Failed to enumerate dv timings info\n");
                 return -EINVAL;
         }
 
@@ -1059,21 +1059,21 @@ vpbe_display_enum_dv_presets(struct file *file, void *priv,
 }
 
 /**
- * vpbe_display_s_dv_preset - Set the dv presets
+ * vpbe_display_s_dv_timings - Set the dv timings
  *
- * Set the preset in the current encoder. Return the status. 0 - success
+ * Set the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
 static int
-vpbe_display_s_dv_preset(struct file *file, void *priv,
-                                struct v4l2_dv_preset *preset)
+vpbe_display_s_dv_timings(struct file *file, void *priv,
+                                struct v4l2_dv_timings *timings)
 {
         struct vpbe_fh *fh = priv;
         struct vpbe_layer *layer = fh->layer;
         struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
         int ret;
 
-        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_S_DV_PRESETS\n");
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_S_DV_TIMINGS\n");
 
 
         /* If streaming is started, return error */
@@ -1083,13 +1083,13 @@ vpbe_display_s_dv_preset(struct file *file, void *priv,
         }
 
         /* Set the given standard in the encoder */
-        if (!vpbe_dev->ops.s_dv_preset)
+        if (!vpbe_dev->ops.s_dv_timings)
                 return -EINVAL;
 
-        ret = vpbe_dev->ops.s_dv_preset(vpbe_dev, preset);
+        ret = vpbe_dev->ops.s_dv_timings(vpbe_dev, timings);
         if (ret) {
                 v4l2_err(&vpbe_dev->v4l2_dev,
-                        "Failed to set the dv presets info\n");
+                        "Failed to set the dv timings info\n");
                 return -EINVAL;
         }
         /* set the current norm to zero to be consistent. If STD is used
@@ -1101,26 +1101,25 @@ vpbe_display_s_dv_preset(struct file *file, void *priv,
 }
 
 /**
- * vpbe_display_g_dv_preset - Set the dv presets
+ * vpbe_display_g_dv_timings - Set the dv timings
  *
- * Get the preset in the current encoder. Return the status. 0 - success
+ * Get the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
 static int
-vpbe_display_g_dv_preset(struct file *file, void *priv,
-                                struct v4l2_dv_preset *dv_preset)
+vpbe_display_g_dv_timings(struct file *file, void *priv,
+                                struct v4l2_dv_timings *dv_timings)
 {
         struct vpbe_fh *fh = priv;
         struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
 
-        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_G_DV_PRESETS\n");
+        v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_G_DV_TIMINGS\n");
 
         /* Get the given standard in the encoder */
 
         if (vpbe_dev->current_timings.timings_type &
-                                VPBE_ENC_DV_PRESET) {
-                dv_preset->preset =
-                        vpbe_dev->current_timings.timings.dv_preset;
+                                VPBE_ENC_CUSTOM_TIMINGS) {
+                *dv_timings = vpbe_dev->current_timings.dv_timings;
         } else {
                 return -EINVAL;
         }
@@ -1572,9 +1571,9 @@ static const struct v4l2_ioctl_ops vpbe_ioctl_ops = {
         .vidioc_enum_output      = vpbe_display_enum_output,
         .vidioc_s_output         = vpbe_display_s_output,
         .vidioc_g_output         = vpbe_display_g_output,
-        .vidioc_s_dv_preset      = vpbe_display_s_dv_preset,
-        .vidioc_g_dv_preset      = vpbe_display_g_dv_preset,
-        .vidioc_enum_dv_presets  = vpbe_display_enum_dv_presets,
+        .vidioc_s_dv_timings     = vpbe_display_s_dv_timings,
+        .vidioc_g_dv_timings     = vpbe_display_g_dv_timings,
+        .vidioc_enum_dv_timings  = vpbe_display_enum_dv_timings,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
         .vidioc_g_register       = vpbe_display_g_register,
         .vidioc_s_register       = vpbe_display_s_register,
@@ -1639,8 +1638,7 @@ static __devinit int init_vpbe_layer(int i, struct vpbe_display *disp_dev,
                         VPBE_ENC_STD) {
                 vbd->tvnorms = (V4L2_STD_525_60 | V4L2_STD_625_50);
                 vbd->current_norm =
-                        disp_dev->vpbe_dev->
-                        current_timings.timings.std_id;
+                        disp_dev->vpbe_dev->current_timings.std_id;
         } else
                 vbd->current_norm = 0;
 
diff --git a/drivers/media/platform/davinci/vpbe_venc.c b/drivers/media/platform/davinci/vpbe_venc.c
index 0302669622d6..aed7369b962a 100644
--- a/drivers/media/platform/davinci/vpbe_venc.c
+++ b/drivers/media/platform/davinci/vpbe_venc.c
@@ -298,7 +298,7 @@ static int venc_set_480p59_94(struct v4l2_subdev *sd)
                 return -EINVAL;
 
         /* Setup clock at VPSS & VENC for SD */
-        if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_480P59_94) < 0)
+        if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 27000000) < 0)
                 return -EINVAL;
 
         venc_enabledigitaloutput(sd, 0);
@@ -345,7 +345,7 @@ static int venc_set_576p50(struct v4l2_subdev *sd)
           (pdata->venc_type != VPBE_VERSION_2))
                 return -EINVAL;
         /* Setup clock at VPSS & VENC for SD */
-        if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_576P50) < 0)
+        if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 27000000) < 0)
                 return -EINVAL;
 
         venc_enabledigitaloutput(sd, 0);
@@ -385,7 +385,7 @@ static int venc_set_720p60_internal(struct v4l2_subdev *sd)
         struct venc_state *venc = to_state(sd);
         struct venc_platform_data *pdata = venc->pdata;
 
-        if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_720P60) < 0)
+        if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 74250000) < 0)
                 return -EINVAL;
 
         venc_enabledigitaloutput(sd, 0);
@@ -413,7 +413,7 @@ static int venc_set_1080i30_internal(struct v4l2_subdev *sd)
         struct venc_state *venc = to_state(sd);
         struct venc_platform_data *pdata = venc->pdata;
 
-        if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_1080P30) < 0)
+        if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 74250000) < 0)
                 return -EINVAL;
 
         venc_enabledigitaloutput(sd, 0);
@@ -446,26 +446,27 @@ static int venc_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
         return -EINVAL;
 }
 
-static int venc_s_dv_preset(struct v4l2_subdev *sd,
-                            struct v4l2_dv_preset *dv_preset)
+static int venc_s_dv_timings(struct v4l2_subdev *sd,
+                            struct v4l2_dv_timings *dv_timings)
 {
         struct venc_state *venc = to_state(sd);
+        u32 height = dv_timings->bt.height;
         int ret;
 
-        v4l2_dbg(debug, 1, sd, "venc_s_dv_preset\n");
+        v4l2_dbg(debug, 1, sd, "venc_s_dv_timings\n");
 
-        if (dv_preset->preset == V4L2_DV_576P50)
+        if (height == 576)
                 return venc_set_576p50(sd);
-        else if (dv_preset->preset == V4L2_DV_480P59_94)
+        else if (height == 480)
                 return venc_set_480p59_94(sd);
-        else if ((dv_preset->preset == V4L2_DV_720P60) &&
+        else if ((height == 720) &&
                         (venc->pdata->venc_type == VPBE_VERSION_2)) {
                 /* TBD setup internal 720p mode here */
                 ret = venc_set_720p60_internal(sd);
                 /* for DM365 VPBE, there is DAC inside */
                 vdaccfg_write(sd, VDAC_CONFIG_HD_V2);
                 return ret;
-        } else if ((dv_preset->preset == V4L2_DV_1080I30) &&
+        } else if ((height == 1080) &&
                 (venc->pdata->venc_type == VPBE_VERSION_2)) {
                 /* TBD setup internal 1080i mode here */
                 ret = venc_set_1080i30_internal(sd);
@@ -518,7 +519,7 @@ static const struct v4l2_subdev_core_ops venc_core_ops = {
 static const struct v4l2_subdev_video_ops venc_video_ops = {
         .s_routing = venc_s_routing,
         .s_std_output = venc_s_std_output,
-        .s_dv_preset = venc_s_dv_preset,
+        .s_dv_timings = venc_s_dv_timings,
 };
 
 static const struct v4l2_subdev_ops venc_ops = {
diff --git a/drivers/media/platform/davinci/vpfe_capture.c b/drivers/media/platform/davinci/vpfe_capture.c
index 48052cbffc2b..8be492cd8ed4 100644
--- a/drivers/media/platform/davinci/vpfe_capture.c
+++ b/drivers/media/platform/davinci/vpfe_capture.c
@@ -1669,6 +1669,7 @@ static int vpfe_s_crop(struct file *file, void *priv,
                              const struct v4l2_crop *crop)
 {
         struct vpfe_device *vpfe_dev = video_drvdata(file);
+        struct v4l2_rect rect = crop->c;
         int ret = 0;
 
         v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_crop\n");
@@ -1684,7 +1685,7 @@ static int vpfe_s_crop(struct file *file, void *priv,
         if (ret)
                 return ret;
 
-        if (crop->c.top < 0 || crop->c.left < 0) {
+        if (rect.top < 0 || rect.left < 0) {
                 v4l2_err(&vpfe_dev->v4l2_dev,
                         "doesn't support negative values for top & left\n");
                 ret = -EINVAL;
@@ -1692,26 +1693,26 @@ static int vpfe_s_crop(struct file *file, void *priv,
         }
 
         /* adjust the width to 16 pixel boundary */
-        crop->c.width = ((crop->c.width + 15) & ~0xf);
+        rect.width = ((rect.width + 15) & ~0xf);
 
         /* make sure parameters are valid */
-        if ((crop->c.left + crop->c.width >
+        if ((rect.left + rect.width >
                 vpfe_dev->std_info.active_pixels) ||
-            (crop->c.top + crop->c.height >
+            (rect.top + rect.height >
                 vpfe_dev->std_info.active_lines)) {
                 v4l2_err(&vpfe_dev->v4l2_dev, "Error in S_CROP params\n");
                 ret = -EINVAL;
                 goto unlock_out;
         }
-        ccdc_dev->hw_ops.set_image_window(&crop->c);
-        vpfe_dev->fmt.fmt.pix.width = crop->c.width;
-        vpfe_dev->fmt.fmt.pix.height = crop->c.height;
+        ccdc_dev->hw_ops.set_image_window(&rect);
+        vpfe_dev->fmt.fmt.pix.width = rect.width;
+        vpfe_dev->fmt.fmt.pix.height = rect.height;
         vpfe_dev->fmt.fmt.pix.bytesperline =
                 ccdc_dev->hw_ops.get_line_length();
         vpfe_dev->fmt.fmt.pix.sizeimage =
                 vpfe_dev->fmt.fmt.pix.bytesperline *
                 vpfe_dev->fmt.fmt.pix.height;
-        vpfe_dev->crop = crop->c;
+        vpfe_dev->crop = rect;
 unlock_out:
         mutex_unlock(&vpfe_dev->lock);
         return ret;
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
index 0bafecac4923..fcabc023885d 100644
--- a/drivers/media/platform/davinci/vpif_capture.c
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -311,12 +311,13 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
         }
 
         /* configure 1 or 2 channel mode */
-        ret = vpif_config_data->setup_input_channel_mode
-                                        (vpif->std_info.ycmux_mode);
-
-        if (ret < 0) {
-                vpif_dbg(1, debug, "can't set vpif channel mode\n");
-                return ret;
+        if (vpif_config_data->setup_input_channel_mode) {
+                ret = vpif_config_data->
+                        setup_input_channel_mode(vpif->std_info.ycmux_mode);
+                if (ret < 0) {
+                        vpif_dbg(1, debug, "can't set vpif channel mode\n");
+                        return ret;
+                }
         }
 
         /* Call vpif_set_params function to set the parameters and addresses */
@@ -863,13 +864,11 @@ static unsigned int vpif_poll(struct file *filep, poll_table * wait)
  */
 static int vpif_open(struct file *filep)
 {
-        struct vpif_capture_config *config = vpif_dev->platform_data;
         struct video_device *vdev = video_devdata(filep);
         struct common_obj *common;
         struct video_obj *vid_ch;
         struct channel_obj *ch;
         struct vpif_fh *fh;
-        int i;
 
         vpif_dbg(2, debug, "vpif_open\n");
 
@@ -878,26 +877,6 @@ static int vpif_open(struct file *filep)
         vid_ch = &ch->video;
         common = &ch->common[VPIF_VIDEO_INDEX];
 
-        if (NULL == ch->curr_subdev_info) {
-                /**
-                 * search through the sub device to see a registered
-                 * sub device and make it as current sub device
-                 */
-                for (i = 0; i < config->subdev_count; i++) {
-                        if (vpif_obj.sd[i]) {
-                                /* the sub device is registered */
-                                ch->curr_subdev_info = &config->subdev_info[i];
-                                /* make first input as the current input */
-                                vid_ch->input_idx = 0;
-                                break;
-                        }
-                }
-                if (i == config->subdev_count) {
-                        vpif_err("No sub device registered\n");
-                        return -ENOENT;
-                }
-        }
-
         /* Allocate memory for the file handle object */
         fh = kzalloc(sizeof(struct vpif_fh), GFP_KERNEL);
         if (NULL == fh) {
@@ -997,6 +976,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
         struct common_obj *common;
         u8 index = 0;
         struct vb2_queue *q;
+        int ret;
 
         vpif_dbg(2, debug, "vpif_reqbufs\n");
 
@@ -1036,8 +1016,12 @@ static int vpif_reqbufs(struct file *file, void *priv,
         q->mem_ops = &vb2_dma_contig_memops;
         q->buf_struct_size = sizeof(struct vpif_cap_buffer);
 
-        vb2_queue_init(q);
-
+        ret = vb2_queue_init(q);
+        if (ret) {
+                vpif_err("vpif_capture: vb2_queue_init() failed\n");
+                vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
+                return ret;
+        }
         /* Set io allowed member of file handle to TRUE */
         fh->io_allowed[index] = 1;
         /* Increment io usrs member of channel object to 1 */
@@ -1175,10 +1159,9 @@ static int vpif_streamon(struct file *file, void *priv,
                 return ret;
 
         /* Enable streamon on the sub device */
-        ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], video,
-                                s_stream, 1);
+        ret = v4l2_subdev_call(ch->sd, video, s_stream, 1);
 
-        if (ret && (ret != -ENOIOCTLCMD)) {
+        if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) {
                 vpif_dbg(1, debug, "stream on failed in subdev\n");
                 return ret;
         }
@@ -1238,73 +1221,105 @@ static int vpif_streamoff(struct file *file, void *priv,
 
         common->started = 0;
 
-        ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], video,
-                                s_stream, 0);
+        ret = v4l2_subdev_call(ch->sd, video, s_stream, 0);
 
-        if (ret && (ret != -ENOIOCTLCMD))
+        if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
                 vpif_dbg(1, debug, "stream off failed in subdev\n");
 
         return vb2_streamoff(&common->buffer_queue, buftype);
 }
 
 /**
- * vpif_map_sub_device_to_input() - Maps sub device to input
- * @ch - ptr to channel
- * @config - ptr to capture configuration
+ * vpif_input_to_subdev() - Maps input to sub device
+ * @vpif_cfg - global config ptr
+ * @chan_cfg - channel config ptr
  * @input_index - Given input index from application
- * @sub_device_index - index into sd table
  *
  * lookup the sub device information for a given input index.
  * we report all the inputs to application. inputs table also
  * has sub device name for the each input
  */
-static struct vpif_subdev_info *vpif_map_sub_device_to_input(
-                                struct channel_obj *ch,
-                                struct vpif_capture_config *vpif_cfg,
-                                int input_index,
-                                int *sub_device_index)
+static int vpif_input_to_subdev(
+                struct vpif_capture_config *vpif_cfg,
+                struct vpif_capture_chan_config *chan_cfg,
+                int input_index)
 {
-        struct vpif_capture_chan_config *chan_cfg;
-        struct vpif_subdev_info *subdev_info = NULL;
-        const char *subdev_name = NULL;
+        struct vpif_subdev_info *subdev_info;
+        const char *subdev_name;
         int i;
 
-        vpif_dbg(2, debug, "vpif_map_sub_device_to_input\n");
+        vpif_dbg(2, debug, "vpif_input_to_subdev\n");
 
-        chan_cfg = &vpif_cfg->chan_config[ch->channel_id];
-
-        /**
-         * search through the inputs to find the sub device supporting
-         * the input
-         */
-        for (i = 0; i < chan_cfg->input_count; i++) {
-                /* For each sub device, loop through input */
-                if (i == input_index) {
-                        subdev_name = chan_cfg->inputs[i].subdev_name;
-                        break;
-                }
-        }
-
-        /* if reached maximum. return null */
-        if (i == chan_cfg->input_count || (NULL == subdev_name))
-                return subdev_info;
+        subdev_name = chan_cfg->inputs[input_index].subdev_name;
+        if (subdev_name == NULL)
+                return -1;
 
         /* loop through the sub device list to get the sub device info */
         for (i = 0; i < vpif_cfg->subdev_count; i++) {
                 subdev_info = &vpif_cfg->subdev_info[i];
                 if (!strcmp(subdev_info->name, subdev_name))
-                        break;
+                        return i;
+        }
+        return -1;
+}
+
+/**
+ * vpif_set_input() - Select an input
+ * @vpif_cfg - global config ptr
+ * @ch - channel
+ * @_index - Given input index from application
+ *
+ * Select the given input.
+ */
+static int vpif_set_input(
+                struct vpif_capture_config *vpif_cfg,
+                struct channel_obj *ch,
+                int index)
+{
+        struct vpif_capture_chan_config *chan_cfg =
+                        &vpif_cfg->chan_config[ch->channel_id];
+        struct vpif_subdev_info *subdev_info = NULL;
+        struct v4l2_subdev *sd = NULL;
+        u32 input = 0, output = 0;
+        int sd_index;
+        int ret;
+
+        sd_index = vpif_input_to_subdev(vpif_cfg, chan_cfg, index);
+        if (sd_index >= 0) {
+                sd = vpif_obj.sd[sd_index];
+                subdev_info = &vpif_cfg->subdev_info[sd_index];
         }
 
-        if (i == vpif_cfg->subdev_count)
-                return subdev_info;
+        /* first setup input path from sub device to vpif */
+        if (sd && vpif_cfg->setup_input_path) {
+                ret = vpif_cfg->setup_input_path(ch->channel_id,
+                                       subdev_info->name);
+                if (ret < 0) {
+                        vpif_dbg(1, debug, "couldn't setup input path for the" \
+                        " sub device %s, for input index %d\n",
+                        subdev_info->name, index);
+                        return ret;
+                }
+        }
 
-        /* check if the sub device is registered */
-        if (NULL == vpif_obj.sd[i])
-                return NULL;
+        if (sd) {
+                input = chan_cfg->inputs[index].input_route;
+                output = chan_cfg->inputs[index].output_route;
+                ret = v4l2_subdev_call(sd, video, s_routing,
+                                input, output, 0);
+                if (ret < 0 && ret != -ENOIOCTLCMD) {
+                        vpif_dbg(1, debug, "Failed to set input\n");
+                        return ret;
+                }
+        }
+        ch->input_idx = index;
+        ch->sd = sd;
+        /* copy interface parameters to vpif */
+        ch->vpifparams.iface = chan_cfg->vpif_if;
 
-        *sub_device_index = i;
-        return subdev_info;
+        /* update tvnorms from the sub device input info */
+        ch->video_dev->tvnorms = chan_cfg->inputs[index].input.std;
+        return 0;
 }
 
 /**
@@ -1324,12 +1339,16 @@ static int vpif_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
         vpif_dbg(2, debug, "vpif_querystd\n");
 
         /* Call querystd function of decoder device */
-        ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], video,
-                                querystd, std_id);
-        if (ret < 0)
-                vpif_dbg(1, debug, "Failed to set standard for sub devices\n");
+        ret = v4l2_subdev_call(ch->sd, video, querystd, std_id);
 
-        return ret;
+        if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+                return -ENODATA;
+        if (ret) {
+                vpif_dbg(1, debug, "Failed to query standard for sub devices\n");
+                return ret;
+        }
+
+        return 0;
 }
 
 /**
@@ -1397,11 +1416,12 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
         vpif_config_format(ch);
 
         /* set standard in the sub device */
-        ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], core,
-                                s_std, *std_id);
-        if (ret < 0)
+        ret = v4l2_subdev_call(ch->sd, core, s_std, *std_id);
+        if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) {
                 vpif_dbg(1, debug, "Failed to set standard for sub devices\n");
-        return ret;
+                return ret;
+        }
+        return 0;
 }
 
 /**
@@ -1441,10 +1461,8 @@ static int vpif_g_input(struct file *file, void *priv, unsigned int *index)
 {
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
-        struct video_obj *vid_ch = &ch->video;
-
-        *index = vid_ch->input_idx;
 
+        *index = ch->input_idx;
         return 0;
 }
 
@@ -1461,13 +1479,13 @@ static int vpif_s_input(struct file *file, void *priv, unsigned int index)
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
         struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-        struct video_obj *vid_ch = &ch->video;
-        struct vpif_subdev_info *subdev_info;
-        int ret = 0, sd_index = 0;
-        u32 input = 0, output = 0;
+        int ret;
 
         chan_cfg = &config->chan_config[ch->channel_id];
 
+        if (index >= chan_cfg->input_count)
+                return -EINVAL;
+
         if (common->started) {
                 vpif_err("Streaming in progress\n");
                 return -EBUSY;
@@ -1486,45 +1504,7 @@ static int vpif_s_input(struct file *file, void *priv, unsigned int index)
                 return ret;
 
         fh->initialized = 1;
-        subdev_info = vpif_map_sub_device_to_input(ch, config, index,
-                                                   &sd_index);
-        if (NULL == subdev_info) {
-                vpif_dbg(1, debug,
-                        "couldn't lookup sub device for the input index\n");
-                return -EINVAL;
-        }
-
-        /* first setup input path from sub device to vpif */
-        if (config->setup_input_path) {
-                ret = config->setup_input_path(ch->channel_id,
-                                               subdev_info->name);
-                if (ret < 0) {
-                        vpif_dbg(1, debug, "couldn't setup input path for the"
-                                " sub device %s, for input index %d\n",
-                                subdev_info->name, index);
-                        return ret;
-                }
-        }
-
-        if (subdev_info->can_route) {
-                input = subdev_info->input;
-                output = subdev_info->output;
-                ret = v4l2_subdev_call(vpif_obj.sd[sd_index], video, s_routing,
-                                        input, output, 0);
-                if (ret < 0) {
-                        vpif_dbg(1, debug, "Failed to set input\n");
-                        return ret;
-                }
-        }
-        vid_ch->input_idx = index;
-        ch->curr_subdev_info = subdev_info;
-        ch->curr_sd_index = sd_index;
-        /* copy interface parameters to vpif */
-        ch->vpifparams.iface = subdev_info->vpif_if;
-
-        /* update tvnorms from the sub device input info */
-        ch->video_dev->tvnorms = chan_cfg->inputs[index].input.std;
-        return ret;
+        return vpif_set_input(config, ch, index);
 }
 
 /**
@@ -1655,9 +1635,11 @@ static int vpif_querycap(struct file *file, void  *priv,
 {
         struct vpif_capture_config *config = vpif_dev->platform_data;
 
-        cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
-        strlcpy(cap->driver, "vpif capture", sizeof(cap->driver));
-        strlcpy(cap->bus_info, "VPIF Platform", sizeof(cap->bus_info));
+        cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+        cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+        snprintf(cap->driver, sizeof(cap->driver), "%s", dev_name(vpif_dev));
+        snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+                 dev_name(vpif_dev));
         strlcpy(cap->card, config->card_name, sizeof(cap->card));
 
         return 0;
@@ -1730,9 +1712,12 @@ vpif_enum_dv_timings(struct file *file, void *priv,
 {
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
+        int ret;
 
-        return v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index],
-                                video, enum_dv_timings, timings);
+        ret = v4l2_subdev_call(ch->sd, video, enum_dv_timings, timings);
+        if (ret == -ENOIOCTLCMD && ret == -ENODEV)
+                return -EINVAL;
+        return ret;
 }
 
 /**
@@ -1747,9 +1732,12 @@ vpif_query_dv_timings(struct file *file, void *priv,
 {
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
+        int ret;
 
-        return v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index],
-                                video, query_dv_timings, timings);
+        ret = v4l2_subdev_call(ch->sd, video, query_dv_timings, timings);
+        if (ret == -ENOIOCTLCMD && ret == -ENODEV)
+                return -ENODATA;
+        return ret;
 }
 
 /**
@@ -1775,13 +1763,9 @@ static int vpif_s_dv_timings(struct file *file, void *priv,
         }
 
         /* Configure subdevice timings, if any */
-        ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index],
-                        video, s_dv_timings, timings);
-        if (ret == -ENOIOCTLCMD) {
-                vpif_dbg(2, debug, "Custom DV timings not supported by "
-                                "subdevice\n");
-                return -EINVAL;
-        }
+        ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings);
+        if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+                ret = 0;
         if (ret < 0) {
                 vpif_dbg(2, debug, "Error setting custom DV timings\n");
                 return ret;
@@ -1906,8 +1890,7 @@ static int vpif_dbg_g_register(struct file *file, void *priv,
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
 
-        return v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], core,
-                        g_register, reg);
+        return v4l2_subdev_call(ch->sd, core, g_register, reg);
 }
 
 /*
@@ -1924,8 +1907,7 @@ static int vpif_dbg_s_register(struct file *file, void *priv,
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
 
-        return v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], core,
-                        s_register, reg);
+        return v4l2_subdev_call(ch->sd, core, s_register, reg);
 }
 #endif
 
@@ -2063,7 +2045,8 @@ static __init int vpif_probe(struct platform_device *pdev)
 {
         struct vpif_subdev_info *subdevdata;
         struct vpif_capture_config *config;
-        int i, j, k, m, q, err;
+        int i, j, k, err;
+        int res_idx = 0;
         struct i2c_adapter *i2c_adap;
         struct channel_obj *ch;
         struct common_obj *common;
@@ -2086,18 +2069,19 @@ static __init int vpif_probe(struct platform_device *pdev)
                 return err;
         }
 
-        k = 0;
-        while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, k))) {
+        while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, res_idx))) {
                 for (i = res->start; i <= res->end; i++) {
                         if (request_irq(i, vpif_channel_isr, IRQF_SHARED,
-                                        "VPIF_Capture",
-                                (void *)(&vpif_obj.dev[k]->channel_id))) {
+                                        "VPIF_Capture", (void *)
+                                        (&vpif_obj.dev[res_idx]->channel_id))) {
                                 err = -EBUSY;
-                                i--;
+                                for (j = 0; j < i; j++)
+                                        free_irq(j, (void *)
+                                        (&vpif_obj.dev[res_idx]->channel_id));
                                 goto vpif_int_err;
                         }
                 }
-                k++;
+                res_idx++;
         }
 
         for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
@@ -2111,7 +2095,7 @@ static __init int vpif_probe(struct platform_device *pdev)
                                 video_device_release(ch->video_dev);
                         }
                         err = -ENOMEM;
-                        goto vpif_dev_alloc_err;
+                        goto vpif_int_err;
                 }
 
                 /* Initialize field of video device */
@@ -2142,24 +2126,6 @@ static __init int vpif_probe(struct platform_device *pdev)
                 }
         }
 
-        for (j = 0; j < VPIF_CAPTURE_MAX_DEVICES; j++) {
-                ch = vpif_obj.dev[j];
-                ch->channel_id = j;
-                common = &(ch->common[VPIF_VIDEO_INDEX]);
-                spin_lock_init(&common->irqlock);
-                mutex_init(&common->lock);
-                ch->video_dev->lock = &common->lock;
-                /* Initialize prio member of channel object */
-                v4l2_prio_init(&ch->prio);
-                err = video_register_device(ch->video_dev,
-                                            VFL_TYPE_GRABBER, (j ? 1 : 0));
-                if (err)
-                        goto probe_out;
-
-                video_set_drvdata(ch->video_dev, ch);
-
-        }
-
         i2c_adap = i2c_get_adapter(1);
         config = pdev->dev.platform_data;
 
@@ -2169,7 +2135,7 @@ static __init int vpif_probe(struct platform_device *pdev)
         if (vpif_obj.sd == NULL) {
                 vpif_err("unable to allocate memory for subdevice pointers\n");
                 err = -ENOMEM;
-                goto probe_out;
+                goto vpif_sd_error;
         }
 
         for (i = 0; i < subdev_count; i++) {
@@ -2186,19 +2152,32 @@ static __init int vpif_probe(struct platform_device *pdev)
                 }
                 v4l2_info(&vpif_obj.v4l2_dev, "registered sub device %s\n",
                           subdevdata->name);
-
-                if (vpif_obj.sd[i])
-                        vpif_obj.sd[i]->grp_id = 1 << i;
         }
 
+        for (j = 0; j < VPIF_CAPTURE_MAX_DEVICES; j++) {
+                ch = vpif_obj.dev[j];
+                ch->channel_id = j;
+                common = &(ch->common[VPIF_VIDEO_INDEX]);
+                spin_lock_init(&common->irqlock);
+                mutex_init(&common->lock);
+                ch->video_dev->lock = &common->lock;
+                /* Initialize prio member of channel object */
+                v4l2_prio_init(&ch->prio);
+                video_set_drvdata(ch->video_dev, ch);
+
+                /* select input 0 */
+                err = vpif_set_input(config, ch, 0);
+                if (err)
+                        goto probe_out;
+
+                err = video_register_device(ch->video_dev,
+                                            VFL_TYPE_GRABBER, (j ? 1 : 0));
+                if (err)
+                        goto probe_out;
+        }
         v4l2_info(&vpif_obj.v4l2_dev, "VPIF capture driver initialized\n");
         return 0;
 
-probe_subdev_out:
-        /* free sub devices memory */
-        kfree(vpif_obj.sd);
-
-        j = VPIF_CAPTURE_MAX_DEVICES;
 probe_out:
         for (k = 0; k < j; k++) {
                 /* Get the pointer to the channel object */
@@ -2206,22 +2185,23 @@ probe_out:
                 /* Unregister video device */
                 video_unregister_device(ch->video_dev);
         }
+probe_subdev_out:
+        /* free sub devices memory */
+        kfree(vpif_obj.sd);
 
-vpif_dev_alloc_err:
-        k = VPIF_CAPTURE_MAX_DEVICES-1;
-        res = platform_get_resource(pdev, IORESOURCE_IRQ, k);
-        i = res->end;
-
-vpif_int_err:
-        for (q = k; q >= 0; q--) {
-                for (m = i; m >= (int)res->start; m--)
-                        free_irq(m, (void *)(&vpif_obj.dev[q]->channel_id));
-
-                res = platform_get_resource(pdev, IORESOURCE_IRQ, q-1);
-                if (res)
-                        i = res->end;
+vpif_sd_error:
+        for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
+                ch = vpif_obj.dev[i];
+                /* Note: does nothing if ch->video_dev == NULL */
+                video_device_release(ch->video_dev);
         }
+vpif_int_err:
         v4l2_device_unregister(&vpif_obj.v4l2_dev);
+        for (i = 0; i < res_idx; i++) {
+                res = platform_get_resource(pdev, IORESOURCE_IRQ, i);
+                for (j = res->start; j <= res->end; j++)
+                        free_irq(j, (void *)(&vpif_obj.dev[i]->channel_id));
+        }
         return err;
 }
 
diff --git a/drivers/media/platform/davinci/vpif_capture.h b/drivers/media/platform/davinci/vpif_capture.h
index d24efc17e4c8..3d3c1e5cd5d4 100644
--- a/drivers/media/platform/davinci/vpif_capture.h
+++ b/drivers/media/platform/davinci/vpif_capture.h
@@ -54,8 +54,6 @@ struct video_obj {
         /* Currently selected or default standard */
         v4l2_std_id stdid;
         struct v4l2_dv_timings dv_timings;
-        /* This is to track the last input that is passed to application */
-        u32 input_idx;
 };
 
 struct vpif_cap_buffer {
@@ -119,10 +117,10 @@ struct channel_obj {
         u8 initialized;
         /* Identifies channel */
         enum vpif_channel_id channel_id;
-        /* index into sd table */
-        int curr_sd_index;
-        /* ptr to current sub device information */
-        struct vpif_subdev_info *curr_subdev_info;
+        /* Current input */
+        u32 input_idx;
+        /* subdev corresponding to the current input, may be NULL */
+        struct v4l2_subdev *sd;
         /* vpif configuration params */
         struct vpif_params vpifparams;
         /* common object array */
@@ -159,10 +157,6 @@ struct vpif_config_params {
         u32 video_limit[VPIF_CAPTURE_NUM_CHANNELS];
         u8 max_device_type;
 };
-/* Struct which keeps track of the line numbers for the sliced vbi service */
-struct vpif_service_line {
-        u16 service_id;
-        u16 service_line[2];
-};
+
 #endif                          /* End of __KERNEL__ */
 #endif                          /* VPIF_CAPTURE_H */
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index a5b88689abad..b716fbd4241f 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -280,12 +280,13 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
         }
 
         /* clock settings */
-        ret =
-            vpif_config_data->set_clock(ch->vpifparams.std_info.ycmux_mode,
-                                        ch->vpifparams.std_info.hd_sd);
-        if (ret < 0) {
-                vpif_err("can't set clock\n");
-                return ret;
+        if (vpif_config_data->set_clock) {
+                ret = vpif_config_data->set_clock(ch->vpifparams.std_info.
+                ycmux_mode, ch->vpifparams.std_info.hd_sd);
+                if (ret < 0) {
+                        vpif_err("can't set clock\n");
+                        return ret;
+                }
         }
 
         /* set the parameters and addresses */
@@ -307,7 +308,7 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
                 channel2_intr_assert();
                 channel2_intr_enable(1);
                 enable_channel2(1);
-                if (vpif_config_data->ch2_clip_en)
+                if (vpif_config_data->chan_config[VPIF_CHANNEL2_VIDEO].clip_en)
                         channel2_clipping_enable(1);
         }
 
@@ -316,7 +317,7 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
                 channel3_intr_assert();
                 channel3_intr_enable(1);
                 enable_channel3(1);
-                if (vpif_config_data->ch3_clip_en)
+                if (vpif_config_data->chan_config[VPIF_CHANNEL3_VIDEO].clip_en)
                         channel3_clipping_enable(1);
         }
 
@@ -826,9 +827,11 @@ static int vpif_querycap(struct file *file, void  *priv,
 {
         struct vpif_display_config *config = vpif_dev->platform_data;
 
-        cap->capabilities = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
-        strlcpy(cap->driver, "vpif display", sizeof(cap->driver));
-        strlcpy(cap->bus_info, "Platform", sizeof(cap->bus_info));
+        cap->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+        cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+        snprintf(cap->driver, sizeof(cap->driver), "%s", dev_name(vpif_dev));
+        snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+                 dev_name(vpif_dev));
         strlcpy(cap->card, config->card_name, sizeof(cap->card));
 
         return 0;
@@ -935,6 +938,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
         enum v4l2_field field;
         struct vb2_queue *q;
         u8 index = 0;
+        int ret;
 
         /* This file handle has not initialized the channel,
            It is not allowed to do settings */
@@ -980,8 +984,12 @@ static int vpif_reqbufs(struct file *file, void *priv,
         q->mem_ops = &vb2_dma_contig_memops;
         q->buf_struct_size = sizeof(struct vpif_disp_buffer);
 
-        vb2_queue_init(q);
-
+        ret = vb2_queue_init(q);
+        if (ret) {
+                vpif_err("vpif_display: vb2_queue_init() failed\n");
+                vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
+                return ret;
+        }
         /* Set io allowed member of file handle to TRUE */
         fh->io_allowed[index] = 1;
         /* Increment io usrs member of channel object to 1 */
@@ -1173,14 +1181,16 @@ static int vpif_streamoff(struct file *file, void *priv,
         if (buftype == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
                 /* disable channel */
                 if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
-                        if (vpif_config_data->ch2_clip_en)
+                        if (vpif_config_data->
+                                chan_config[VPIF_CHANNEL2_VIDEO].clip_en)
                                 channel2_clipping_enable(0);
                         enable_channel2(0);
                         channel2_intr_enable(0);
                 }
                 if ((VPIF_CHANNEL3_VIDEO == ch->channel_id) ||
                                         (2 == common->started)) {
-                        if (vpif_config_data->ch3_clip_en)
+                        if (vpif_config_data->
+                                chan_config[VPIF_CHANNEL3_VIDEO].clip_en)
                                 channel3_clipping_enable(0);
                         enable_channel3(0);
                         channel3_intr_enable(0);
@@ -1213,49 +1223,126 @@ static int vpif_enum_output(struct file *file, void *fh,
 {
 
         struct vpif_display_config *config = vpif_dev->platform_data;
+        struct vpif_display_chan_config *chan_cfg;
+        struct vpif_fh *vpif_handler = fh;
+        struct channel_obj *ch = vpif_handler->channel;
 
-        if (output->index >= config->output_count) {
+        chan_cfg = &config->chan_config[ch->channel_id];
+        if (output->index >= chan_cfg->output_count) {
                 vpif_dbg(1, debug, "Invalid output index\n");
                 return -EINVAL;
         }
 
-        strcpy(output->name, config->output[output->index]);
-        output->type = V4L2_OUTPUT_TYPE_ANALOG;
-        output->std = VPIF_V4L2_STD;
+        *output = chan_cfg->outputs[output->index].output;
+        return 0;
+}
+
+/**
+ * vpif_output_to_subdev() - Maps output to sub device
+ * @vpif_cfg - global config ptr
+ * @chan_cfg - channel config ptr
+ * @index - Given output index from application
+ *
+ * lookup the sub device information for a given output index.
+ * we report all the output to application. output table also
+ * has sub device name for the each output
+ */
+static int
+vpif_output_to_subdev(struct vpif_display_config *vpif_cfg,
+                      struct vpif_display_chan_config *chan_cfg, int index)
+{
+        struct vpif_subdev_info *subdev_info;
+        const char *subdev_name;
+        int i;
+
+        vpif_dbg(2, debug, "vpif_output_to_subdev\n");
+
+        if (chan_cfg->outputs == NULL)
+                return -1;
+
+        subdev_name = chan_cfg->outputs[index].subdev_name;
+        if (subdev_name == NULL)
+                return -1;
+
+        /* loop through the sub device list to get the sub device info */
+        for (i = 0; i < vpif_cfg->subdev_count; i++) {
+                subdev_info = &vpif_cfg->subdevinfo[i];
+                if (!strcmp(subdev_info->name, subdev_name))
+                        return i;
+        }
+        return -1;
+}
+
+/**
+ * vpif_set_output() - Select an output
+ * @vpif_cfg - global config ptr
+ * @ch - channel
+ * @index - Given output index from application
+ *
+ * Select the given output.
+ */
+static int vpif_set_output(struct vpif_display_config *vpif_cfg,
+                      struct channel_obj *ch, int index)
+{
+        struct vpif_display_chan_config *chan_cfg =
+                &vpif_cfg->chan_config[ch->channel_id];
+        struct vpif_subdev_info *subdev_info = NULL;
+        struct v4l2_subdev *sd = NULL;
+        u32 input = 0, output = 0;
+        int sd_index;
+        int ret;
+
+        sd_index = vpif_output_to_subdev(vpif_cfg, chan_cfg, index);
+        if (sd_index >= 0) {
+                sd = vpif_obj.sd[sd_index];
+                subdev_info = &vpif_cfg->subdevinfo[sd_index];
+        }
+
+        if (sd) {
+                input = chan_cfg->outputs[index].input_route;
+                output = chan_cfg->outputs[index].output_route;
+                ret = v4l2_subdev_call(sd, video, s_routing, input, output, 0);
+                if (ret < 0 && ret != -ENOIOCTLCMD) {
+                        vpif_err("Failed to set output\n");
+                        return ret;
+                }
 
+        }
+        ch->output_idx = index;
+        ch->sd = sd;
+        if (chan_cfg->outputs != NULL)
+                /* update tvnorms from the sub device output info */
+                ch->video_dev->tvnorms = chan_cfg->outputs[index].output.std;
         return 0;
 }
 
 static int vpif_s_output(struct file *file, void *priv, unsigned int i)
 {
+        struct vpif_display_config *config = vpif_dev->platform_data;
+        struct vpif_display_chan_config *chan_cfg;
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
-        struct video_obj *vid_ch = &ch->video;
         struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-        int ret = 0;
+
+        chan_cfg = &config->chan_config[ch->channel_id];
+
+        if (i >= chan_cfg->output_count)
+                return -EINVAL;
 
         if (common->started) {
                 vpif_err("Streaming in progress\n");
                 return -EBUSY;
         }
 
-        ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video,
-                                                        s_routing, 0, i, 0);
-
-        if (ret < 0)
-                vpif_err("Failed to set output standard\n");
-
-        vid_ch->output_id = i;
-        return ret;
+        return vpif_set_output(config, ch, i);
 }
 
 static int vpif_g_output(struct file *file, void *priv, unsigned int *i)
 {
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
-        struct video_obj *vid_ch = &ch->video;
 
-        *i = vid_ch->output_id;
+        *i = ch->output_idx;
 
         return 0;
 }
@@ -1290,10 +1377,12 @@ vpif_enum_dv_timings(struct file *file, void *priv,
 {
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
-        struct video_obj *vid_ch = &ch->video;
+        int ret;
 
-        return v4l2_subdev_call(vpif_obj.sd[vid_ch->output_id],
-                        video, enum_dv_timings, timings);
+        ret = v4l2_subdev_call(ch->sd, video, enum_dv_timings, timings);
+        if (ret == -ENOIOCTLCMD && ret == -ENODEV)
+                return -EINVAL;
+        return ret;
 }
 
 /**
@@ -1319,13 +1408,9 @@ static int vpif_s_dv_timings(struct file *file, void *priv,
         }
 
         /* Configure subdevice timings, if any */
-        ret = v4l2_subdev_call(vpif_obj.sd[vid_ch->output_id],
-                        video, s_dv_timings, timings);
-        if (ret == -ENOIOCTLCMD) {
-                vpif_dbg(2, debug, "Custom DV timings not supported by "
-                                "subdevice\n");
-                return -EINVAL;
-        }
+        ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings);
+        if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+                ret = 0;
         if (ret < 0) {
                 vpif_dbg(2, debug, "Error setting custom DV timings\n");
                 return ret;
@@ -1450,10 +1535,8 @@ static int vpif_dbg_g_register(struct file *file, void *priv,
                 struct v4l2_dbg_register *reg){
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
-        struct video_obj *vid_ch = &ch->video;
 
-        return v4l2_subdev_call(vpif_obj.sd[vid_ch->output_id], core,
-                        g_register, reg);
+        return v4l2_subdev_call(ch->sd, core, g_register, reg);
 }
 
 /*
@@ -1469,10 +1552,8 @@ static int vpif_dbg_s_register(struct file *file, void *priv,
                 struct v4l2_dbg_register *reg){
         struct vpif_fh *fh = priv;
         struct channel_obj *ch = fh->channel;
-        struct video_obj *vid_ch = &ch->video;
 
-        return v4l2_subdev_call(vpif_obj.sd[vid_ch->output_id], core,
-                        s_register, reg);
+        return v4l2_subdev_call(ch->sd, core, s_register, reg);
 }
 #endif
 
@@ -1536,9 +1617,6 @@ static struct video_device vpif_video_template = {
         .name           = "vpif",
         .fops           = &vpif_fops,
         .ioctl_ops      = &vpif_ioctl_ops,
-        .tvnorms        = VPIF_V4L2_STD,
-        .current_norm   = V4L2_STD_625_50,
-
 };
 
 /*Configure the channels, buffer sizei, request irq */
@@ -1611,7 +1689,8 @@ static __init int vpif_probe(struct platform_device *pdev)
 {
         struct vpif_subdev_info *subdevdata;
         struct vpif_display_config *config;
-        int i, j = 0, k, q, m, err = 0;
+        int i, j = 0, k, err = 0;
+        int res_idx = 0;
         struct i2c_adapter *i2c_adap;
         struct common_obj *common;
         struct channel_obj *ch;
@@ -1634,21 +1713,22 @@ static __init int vpif_probe(struct platform_device *pdev)
                 return err;
         }
 
-        k = 0;
-        while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, k))) {
+        while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, res_idx))) {
                 for (i = res->start; i <= res->end; i++) {
                         if (request_irq(i, vpif_channel_isr, IRQF_SHARED,
-                                        "VPIF_Display",
-                                (void *)(&vpif_obj.dev[k]->channel_id))) {
+                                        "VPIF_Display", (void *)
+                                        (&vpif_obj.dev[res_idx]->channel_id))) {
                                 err = -EBUSY;
+                                for (j = 0; j < i; j++)
+                                        free_irq(j, (void *)
+                                        (&vpif_obj.dev[res_idx]->channel_id));
                                 goto vpif_int_err;
                         }
                 }
-                k++;
+                res_idx++;
         }
 
         for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
-
                 /* Get the pointer to the channel object */
                 ch = vpif_obj.dev[i];
 
@@ -1694,6 +1774,32 @@ static __init int vpif_probe(struct platform_device *pdev)
                 }
         }
 
+        i2c_adap = i2c_get_adapter(1);
+        config = pdev->dev.platform_data;
+        subdev_count = config->subdev_count;
+        subdevdata = config->subdevinfo;
+        vpif_obj.sd = kzalloc(sizeof(struct v4l2_subdev *) * subdev_count,
+                                                                GFP_KERNEL);
+        if (vpif_obj.sd == NULL) {
+                vpif_err("unable to allocate memory for subdevice pointers\n");
+                err = -ENOMEM;
+                goto vpif_sd_error;
+        }
+
+        for (i = 0; i < subdev_count; i++) {
+                vpif_obj.sd[i] = v4l2_i2c_new_subdev_board(&vpif_obj.v4l2_dev,
+                                                i2c_adap,
+                                                &subdevdata[i].board_info,
+                                                NULL);
+                if (!vpif_obj.sd[i]) {
+                        vpif_err("Error registering v4l2 subdevice\n");
+                        goto probe_subdev_out;
+                }
+
+                if (vpif_obj.sd[i])
+                        vpif_obj.sd[i]->grp_id = 1 << i;
+        }
+
         for (j = 0; j < VPIF_DISPLAY_MAX_DEVICES; j++) {
                 ch = vpif_obj.dev[j];
                 /* Initialize field of the channel objects */
@@ -1715,6 +1821,8 @@ static __init int vpif_probe(struct platform_device *pdev)
 
                 }
                 ch->initialized = 0;
+                if (subdev_count)
+                        ch->sd = vpif_obj.sd[0];
                 ch->channel_id = j;
                 if (j < 2)
                         ch->common[VPIF_VIDEO_INDEX].numbuffers =
@@ -1729,6 +1837,12 @@ static __init int vpif_probe(struct platform_device *pdev)
                 ch->common[VPIF_VIDEO_INDEX].fmt.type =
                                                 V4L2_BUF_TYPE_VIDEO_OUTPUT;
                 ch->video_dev->lock = &common->lock;
+                video_set_drvdata(ch->video_dev, ch);
+
+                /* select output 0 */
+                err = vpif_set_output(config, ch, 0);
+                if (err)
+                        goto probe_out;
 
                 /* register video device */
                 vpif_dbg(1, debug, "channel=%x,channel->video_dev=%x\n",
@@ -1738,42 +1852,12 @@ static __init int vpif_probe(struct platform_device *pdev)
                                           VFL_TYPE_GRABBER, (j ? 3 : 2));
                 if (err < 0)
                         goto probe_out;
-
-                video_set_drvdata(ch->video_dev, ch);
-        }
-
-        i2c_adap = i2c_get_adapter(1);
-        config = pdev->dev.platform_data;
-        subdev_count = config->subdev_count;
-        subdevdata = config->subdevinfo;
-        vpif_obj.sd = kzalloc(sizeof(struct v4l2_subdev *) * subdev_count,
-                                                                GFP_KERNEL);
-        if (vpif_obj.sd == NULL) {
-                vpif_err("unable to allocate memory for subdevice pointers\n");
-                err = -ENOMEM;
-                goto probe_out;
-        }
-
-        for (i = 0; i < subdev_count; i++) {
-                vpif_obj.sd[i] = v4l2_i2c_new_subdev_board(&vpif_obj.v4l2_dev,
-                                                i2c_adap,
-                                                &subdevdata[i].board_info,
-                                                NULL);
-                if (!vpif_obj.sd[i]) {
-                        vpif_err("Error registering v4l2 subdevice\n");
-                        goto probe_subdev_out;
-                }
-
-                if (vpif_obj.sd[i])
-                        vpif_obj.sd[i]->grp_id = 1 << i;
         }
 
         v4l2_info(&vpif_obj.v4l2_dev,
                         " VPIF display driver initialized\n");
         return 0;
 
-probe_subdev_out:
-        kfree(vpif_obj.sd);
 probe_out:
         for (k = 0; k < j; k++) {
                 ch = vpif_obj.dev[k];
@@ -1781,14 +1865,21 @@ probe_out:
                 video_device_release(ch->video_dev);
                 ch->video_dev = NULL;
         }
+probe_subdev_out:
+        kfree(vpif_obj.sd);
+vpif_sd_error:
+        for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
+                ch = vpif_obj.dev[i];
+                /* Note: does nothing if ch->video_dev == NULL */
+                video_device_release(ch->video_dev);
+        }
 vpif_int_err:
         v4l2_device_unregister(&vpif_obj.v4l2_dev);
         vpif_err("VPIF IRQ request failed\n");
-        for (q = k; k >= 0; k--) {
-                for (m = i; m >= res->start; m--)
-                        free_irq(m, (void *)(&vpif_obj.dev[k]->channel_id));
-                res = platform_get_resource(pdev, IORESOURCE_IRQ, k-1);
-                m = res->end;
+        for (i = 0; i < res_idx; i++) {
+                res = platform_get_resource(pdev, IORESOURCE_IRQ, i);
+                for (j = res->start; j <= res->end; j++)
+                        free_irq(j, (void *)(&vpif_obj.dev[i]->channel_id));
         }
 
         return err;
diff --git a/drivers/media/platform/davinci/vpif_display.h b/drivers/media/platform/davinci/vpif_display.h
index f628ebcf3674..a5a18f74395c 100644
--- a/drivers/media/platform/davinci/vpif_display.h
+++ b/drivers/media/platform/davinci/vpif_display.h
@@ -62,13 +62,6 @@ struct video_obj {
         v4l2_std_id stdid;              /* Currently selected or default
                                          * standard */
         struct v4l2_dv_timings dv_timings;
-        u32 output_id;                  /* Current output id */
-};
-
-struct vbi_obj {
-        int num_services;
-        struct vpif_vbi_params vbiparams;       /* vpif parameters for the raw
-                                                 * vbi data */
 };
 
 struct vpif_disp_buffer {
@@ -131,12 +124,13 @@ struct channel_obj {
                                          * which is being displayed */
         u8 initialized;                 /* flag to indicate whether
                                          * encoder is initialized */
+        u32 output_idx;                 /* Current output index */
+        struct v4l2_subdev *sd;         /* Current output subdev(may be NULL) */
 
         enum vpif_channel_id channel_id;/* Identifies channel */
         struct vpif_params vpifparams;
         struct common_obj common[VPIF_NUMOBJECTS];
         struct video_obj video;
-        struct vbi_obj vbi;
 };
 
 /* File handle structure */
@@ -168,12 +162,4 @@ struct vpif_config_params {
         u8 min_numbuffers;
 };
 
-/* Struct which keeps track of the line numbers for the sliced vbi service */
-struct vpif_service_line {
-        u16 service_id;
-        u16 service_line[2];
-        u16 enc_service_id;
-        u8 bytestowrite;
-};
-
 #endif                          /* DAVINCIHD_DISPLAY_H */
diff --git a/drivers/media/platform/exynos-gsc/gsc-regs.c b/drivers/media/platform/exynos-gsc/gsc-regs.c
index 0d8625f03a32..0146b354dc22 100644
--- a/drivers/media/platform/exynos-gsc/gsc-regs.c
+++ b/drivers/media/platform/exynos-gsc/gsc-regs.c
@@ -212,7 +212,7 @@ void gsc_hw_set_in_image_format(struct gsc_ctx *ctx)
                 else
                         cfg |= GSC_IN_YUV422_3P;
                 break;
-        };
+        }
 
         writel(cfg, dev->regs + GSC_IN_CON);
 }
@@ -332,7 +332,7 @@ void gsc_hw_set_out_image_format(struct gsc_ctx *ctx)
         case 3:
                 cfg |= GSC_OUT_YUV420_3P;
                 break;
-        };
+        }
 
 end_set:
         writel(cfg, dev->regs + GSC_OUT_CON);
diff --git a/drivers/media/platform/fsl-viu.c b/drivers/media/platform/fsl-viu.c
index 897250b88647..31ac4dc69247 100644
--- a/drivers/media/platform/fsl-viu.c
+++ b/drivers/media/platform/fsl-viu.c
@@ -864,7 +864,7 @@ int vidioc_s_fbuf(struct file *file, void *priv, const struct v4l2_framebuffer *
 {
         struct viu_fh  *fh = priv;
         struct viu_dev *dev = fh->dev;
-        struct v4l2_framebuffer *fb = arg;
+        const struct v4l2_framebuffer *fb = arg;
         struct viu_fmt *fmt;
 
         if (!capable(CAP_SYS_ADMIN) && !capable(CAP_SYS_RAWIO))
diff --git a/drivers/media/platform/mem2mem_testdev.c b/drivers/media/platform/mem2mem_testdev.c
index d03637537118..2e2121e98133 100644
--- a/drivers/media/platform/mem2mem_testdev.c
+++ b/drivers/media/platform/mem2mem_testdev.c
@@ -397,8 +397,7 @@ static void device_isr(unsigned long priv)
         curr_ctx = v4l2_m2m_get_curr_priv(m2mtest_dev->m2m_dev);
 
         if (NULL == curr_ctx) {
-                printk(KERN_ERR
-                        "Instance released before the end of transaction\n");
+                pr_err("Instance released before the end of transaction\n");
                 return;
         }
 
@@ -894,7 +893,7 @@ static int m2mtest_open(struct file *file)
 
         if (mutex_lock_interruptible(&dev->dev_mutex))
                 return -ERESTARTSYS;
-        ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
+        ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
         if (!ctx) {
                 rc = -ENOMEM;
                 goto open_unlock;
@@ -1020,7 +1019,7 @@ static int m2mtest_probe(struct platform_device *pdev)
         struct video_device *vfd;
         int ret;
 
-        dev = kzalloc(sizeof *dev, GFP_KERNEL);
+        dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
         if (!dev)
                 return -ENOMEM;
 
@@ -1028,7 +1027,7 @@ static int m2mtest_probe(struct platform_device *pdev)
 
         ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
         if (ret)
-                goto free_dev;
+                return ret;
 
         atomic_set(&dev->num_inst, 0);
         mutex_init(&dev->dev_mutex);
@@ -1067,15 +1066,13 @@ static int m2mtest_probe(struct platform_device *pdev)
 
         return 0;
 
-        v4l2_m2m_release(dev->m2m_dev);
 err_m2m:
+        v4l2_m2m_release(dev->m2m_dev);
         video_unregister_device(dev->vfd);
 rel_vdev:
         video_device_release(vfd);
 unreg_dev:
         v4l2_device_unregister(&dev->v4l2_dev);
-free_dev:
-        kfree(dev);
 
         return ret;
 }
@@ -1090,7 +1087,6 @@ static int m2mtest_remove(struct platform_device *pdev)
         del_timer_sync(&dev->timer);
         video_unregister_device(dev->vfd);
         v4l2_device_unregister(&dev->v4l2_dev);
-        kfree(dev);
 
         return 0;
 }
diff --git a/drivers/media/platform/omap3isp/ispreg.h b/drivers/media/platform/omap3isp/ispreg.h
index 084ea77d65a7..e2c57f334c5d 100644
--- a/drivers/media/platform/omap3isp/ispreg.h
+++ b/drivers/media/platform/omap3isp/ispreg.h
@@ -27,13 +27,13 @@
 #ifndef OMAP3_ISP_REG_H
 #define OMAP3_ISP_REG_H
 
-#include <plat/omap34xx.h>
-
-
 #define CM_CAM_MCLK_HZ                  172800000       /* Hz */
 
 /* ISP Submodules offset */
 
+#define L4_34XX_BASE                    0x48000000
+#define OMAP3430_ISP_BASE               (L4_34XX_BASE + 0xBC000)
+
 #define OMAP3ISP_REG_BASE               OMAP3430_ISP_BASE
 #define OMAP3ISP_REG(offset)            (OMAP3ISP_REG_BASE + (offset))
 
diff --git a/drivers/media/platform/s5p-fimc/fimc-capture.c b/drivers/media/platform/s5p-fimc/fimc-capture.c
index dded98815220..367efd164d0f 100644
--- a/drivers/media/platform/s5p-fimc/fimc-capture.c
+++ b/drivers/media/platform/s5p-fimc/fimc-capture.c
@@ -177,7 +177,9 @@ static int fimc_capture_config_update(struct fimc_ctx *ctx)
 
 void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf)
 {
+        struct v4l2_subdev *csis = fimc->pipeline.subdevs[IDX_CSIS];
         struct fimc_vid_cap *cap = &fimc->vid_cap;
+        struct fimc_frame *f = &cap->ctx->d_frame;
         struct fimc_vid_buffer *v_buf;
         struct timeval *tv;
         struct timespec ts;
@@ -216,6 +218,25 @@ void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf)
                 if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
                         cap->buf_index = 0;
         }
+        /*
+         * Set up a buffer at MIPI-CSIS if current image format
+         * requires the frame embedded data capture.
+         */
+        if (f->fmt->mdataplanes && !list_empty(&cap->active_buf_q)) {
+                unsigned int plane = ffs(f->fmt->mdataplanes) - 1;
+                unsigned int size = f->payload[plane];
+                s32 index = fimc_hw_get_frame_index(fimc);
+                void *vaddr;
+
+                list_for_each_entry(v_buf, &cap->active_buf_q, list) {
+                        if (v_buf->index != index)
+                                continue;
+                        vaddr = vb2_plane_vaddr(&v_buf->vb, plane);
+                        v4l2_subdev_call(csis, video, s_rx_buffer,
+                                         vaddr, &size);
+                        break;
+                }
+        }
 
         if (cap->active_buf_cnt == 0) {
                 if (deq_buf)
@@ -351,6 +372,8 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
                 unsigned int size = (wh * fmt->depth[i]) / 8;
                 if (pixm)
                         sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
+                else if (fimc_fmt_is_user_defined(fmt->color))
+                        sizes[i] = frame->payload[i];
                 else
                         sizes[i] = max_t(u32, size, frame->payload[i]);
 
@@ -611,10 +634,10 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
         u32 mask = FMT_FLAGS_CAM;
         struct fimc_fmt *ffmt;
 
-        /* Color conversion from/to JPEG is not supported */
+        /* Conversion from/to JPEG or User Defined format is not supported */
         if (code && ctx->s_frame.fmt && pad == FIMC_SD_PAD_SOURCE &&
-            fimc_fmt_is_jpeg(ctx->s_frame.fmt->color))
-                *code = V4L2_MBUS_FMT_JPEG_1X8;
+            fimc_fmt_is_user_defined(ctx->s_frame.fmt->color))
+                *code = ctx->s_frame.fmt->mbus_code;
 
         if (fourcc && *fourcc != V4L2_PIX_FMT_JPEG && pad != FIMC_SD_PAD_SINK)
                 mask |= FMT_FLAGS_M2M;
@@ -628,18 +651,19 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
                 *fourcc = ffmt->fourcc;
 
         if (pad == FIMC_SD_PAD_SINK) {
-                max_w = fimc_fmt_is_jpeg(ffmt->color) ?
+                max_w = fimc_fmt_is_user_defined(ffmt->color) ?
                         pl->scaler_dis_w : pl->scaler_en_w;
                 /* Apply the camera input interface pixel constraints */
                 v4l_bound_align_image(width, max_t(u32, *width, 32), max_w, 4,
                                       height, max_t(u32, *height, 32),
                                       FIMC_CAMIF_MAX_HEIGHT,
-                                      fimc_fmt_is_jpeg(ffmt->color) ? 3 : 1,
+                                      fimc_fmt_is_user_defined(ffmt->color) ?
+                                      3 : 1,
                                       0);
                 return ffmt;
         }
         /* Can't scale or crop in transparent (JPEG) transfer mode */
-        if (fimc_fmt_is_jpeg(ffmt->color)) {
+        if (fimc_fmt_is_user_defined(ffmt->color)) {
                 *width  = ctx->s_frame.f_width;
                 *height = ctx->s_frame.f_height;
                 return ffmt;
@@ -684,7 +708,7 @@ static void fimc_capture_try_selection(struct fimc_ctx *ctx,
         u32 max_sc_h, max_sc_v;
 
         /* In JPEG transparent transfer mode cropping is not supported */
-        if (fimc_fmt_is_jpeg(ctx->d_frame.fmt->color)) {
+        if (fimc_fmt_is_user_defined(ctx->d_frame.fmt->color)) {
                 r->width  = sink->f_width;
                 r->height = sink->f_height;
                 r->left   = r->top = 0;
@@ -847,6 +871,48 @@ static int fimc_pipeline_try_format(struct fimc_ctx *ctx,
         return 0;
 }
 
+/**
+ * fimc_get_sensor_frame_desc - query the sensor for media bus frame parameters
+ * @sensor: pointer to the sensor subdev
+ * @plane_fmt: provides plane sizes corresponding to the frame layout entries
+ * @try: true to set the frame parameters, false to query only
+ *
+ * This function is used by this driver only for compressed/blob data formats.
+ */
+static int fimc_get_sensor_frame_desc(struct v4l2_subdev *sensor,
+                                      struct v4l2_plane_pix_format *plane_fmt,
+                                      unsigned int num_planes, bool try)
+{
+        struct v4l2_mbus_frame_desc fd;
+        int i, ret;
+
+        for (i = 0; i < num_planes; i++)
+                fd.entry[i].length = plane_fmt[i].sizeimage;
+
+        if (try)
+                ret = v4l2_subdev_call(sensor, pad, set_frame_desc, 0, &fd);
+        else
+                ret = v4l2_subdev_call(sensor, pad, get_frame_desc, 0, &fd);
+
+        if (ret < 0)
+                return ret;
+
+        if (num_planes != fd.num_entries)
+                return -EINVAL;
+
+        for (i = 0; i < num_planes; i++)
+                plane_fmt[i].sizeimage = fd.entry[i].length;
+
+        if (fd.entry[0].length > FIMC_MAX_JPEG_BUF_SIZE) {
+                v4l2_err(sensor->v4l2_dev,  "Unsupported buffer size: %u\n",
+                         fd.entry[0].length);
+
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
 static int fimc_cap_g_fmt_mplane(struct file *file, void *fh,
                                  struct v4l2_format *f)
 {
@@ -865,7 +931,7 @@ static int fimc_cap_try_fmt_mplane(struct file *file, void *fh,
         struct v4l2_mbus_framefmt mf;
         struct fimc_fmt *ffmt = NULL;
 
-        if (pix->pixelformat == V4L2_PIX_FMT_JPEG) {
+        if (fimc_jpeg_fourcc(pix->pixelformat)) {
                 fimc_capture_try_format(ctx, &pix->width, &pix->height,
                                         NULL, &pix->pixelformat,
                                         FIMC_SD_PAD_SINK);
@@ -879,25 +945,32 @@ static int fimc_cap_try_fmt_mplane(struct file *file, void *fh,
                 return -EINVAL;
 
         if (!fimc->vid_cap.user_subdev_api) {
-                mf.width  = pix->width;
+                mf.width = pix->width;
                 mf.height = pix->height;
-                mf.code   = ffmt->mbus_code;
+                mf.code = ffmt->mbus_code;
                 fimc_md_graph_lock(fimc);
                 fimc_pipeline_try_format(ctx, &mf, &ffmt, false);
                 fimc_md_graph_unlock(fimc);
-
-                pix->width       = mf.width;
-                pix->height      = mf.height;
+                pix->width = mf.width;
+                pix->height = mf.height;
                 if (ffmt)
                         pix->pixelformat = ffmt->fourcc;
         }
 
         fimc_adjust_mplane_format(ffmt, pix->width, pix->height, pix);
+
+        if (ffmt->flags & FMT_FLAGS_COMPRESSED)
+                fimc_get_sensor_frame_desc(fimc->pipeline.subdevs[IDX_SENSOR],
+                                        pix->plane_fmt, ffmt->memplanes, true);
+
         return 0;
 }
 
-static void fimc_capture_mark_jpeg_xfer(struct fimc_ctx *ctx, bool jpeg)
+static void fimc_capture_mark_jpeg_xfer(struct fimc_ctx *ctx,
+                                        enum fimc_color_fmt color)
 {
+        bool jpeg = fimc_fmt_is_user_defined(color);
+
         ctx->scaler.enabled = !jpeg;
         fimc_ctrls_activate(ctx, !jpeg);
 
@@ -920,7 +993,7 @@ static int fimc_capture_set_format(struct fimc_dev *fimc, struct v4l2_format *f)
                 return -EBUSY;
 
         /* Pre-configure format at camera interface input, for JPEG only */
-        if (pix->pixelformat == V4L2_PIX_FMT_JPEG) {
+        if (fimc_jpeg_fourcc(pix->pixelformat)) {
                 fimc_capture_try_format(ctx, &pix->width, &pix->height,
                                         NULL, &pix->pixelformat,
                                         FIMC_SD_PAD_SINK);
@@ -953,7 +1026,16 @@ static int fimc_capture_set_format(struct fimc_dev *fimc, struct v4l2_format *f)
         }
 
         fimc_adjust_mplane_format(ff->fmt, pix->width, pix->height, pix);
-        for (i = 0; i < ff->fmt->colplanes; i++)
+
+        if (ff->fmt->flags & FMT_FLAGS_COMPRESSED) {
+                ret = fimc_get_sensor_frame_desc(fimc->pipeline.subdevs[IDX_SENSOR],
+                                        pix->plane_fmt, ff->fmt->memplanes,
+                                        true);
+                if (ret < 0)
+                        return ret;
+        }
+
+        for (i = 0; i < ff->fmt->memplanes; i++)
                 ff->payload[i] = pix->plane_fmt[i].sizeimage;
 
         set_frame_bounds(ff, pix->width, pix->height);
@@ -961,7 +1043,7 @@ static int fimc_capture_set_format(struct fimc_dev *fimc, struct v4l2_format *f)
         if (!(ctx->state & FIMC_COMPOSE))
                 set_frame_crop(ff, 0, 0, pix->width, pix->height);
 
-        fimc_capture_mark_jpeg_xfer(ctx, fimc_fmt_is_jpeg(ff->fmt->color));
+        fimc_capture_mark_jpeg_xfer(ctx, ff->fmt->color);
 
         /* Reset cropping and set format at the camera interface input */
         if (!fimc->vid_cap.user_subdev_api) {
@@ -1063,6 +1145,23 @@ static int fimc_pipeline_validate(struct fimc_dev *fimc)
                     src_fmt.format.height != sink_fmt.format.height ||
                     src_fmt.format.code != sink_fmt.format.code)
                         return -EPIPE;
+
+                if (sd == fimc->pipeline.subdevs[IDX_SENSOR] &&
+                    fimc_user_defined_mbus_fmt(src_fmt.format.code)) {
+                        struct v4l2_plane_pix_format plane_fmt[FIMC_MAX_PLANES];
+                        struct fimc_frame *frame = &vid_cap->ctx->d_frame;
+                        unsigned int i;
+
+                        ret = fimc_get_sensor_frame_desc(sd, plane_fmt,
+                                                         frame->fmt->memplanes,
+                                                         false);
+                        if (ret < 0)
+                                return -EPIPE;
+
+                        for (i = 0; i < frame->fmt->memplanes; i++)
+                                if (frame->payload[i] < plane_fmt[i].sizeimage)
+                                        return -EPIPE;
+                }
         }
         return 0;
 }
@@ -1424,7 +1523,7 @@ static int fimc_subdev_set_fmt(struct v4l2_subdev *sd,
         /* Update RGB Alpha control state and value range */
         fimc_alpha_ctrl_update(ctx);
 
-        fimc_capture_mark_jpeg_xfer(ctx, fimc_fmt_is_jpeg(ffmt->color));
+        fimc_capture_mark_jpeg_xfer(ctx, ffmt->color);
 
         ff = fmt->pad == FIMC_SD_PAD_SINK ?
                 &ctx->s_frame : &ctx->d_frame;
diff --git a/drivers/media/platform/s5p-fimc/fimc-core.c b/drivers/media/platform/s5p-fimc/fimc-core.c
index 1a445404e73d..8d0d2b94a135 100644
--- a/drivers/media/platform/s5p-fimc/fimc-core.c
+++ b/drivers/media/platform/s5p-fimc/fimc-core.c
@@ -184,7 +184,17 @@ static struct fimc_fmt fimc_formats[] = {
                 .memplanes      = 1,
                 .colplanes      = 1,
                 .mbus_code      = V4L2_MBUS_FMT_JPEG_1X8,
-                .flags          = FMT_FLAGS_CAM,
+                .flags          = FMT_FLAGS_CAM | FMT_FLAGS_COMPRESSED,
+        }, {
+                .name           = "S5C73MX interleaved UYVY/JPEG",
+                .fourcc         = V4L2_PIX_FMT_S5C_UYVY_JPG,
+                .color          = FIMC_FMT_YUYV_JPEG,
+                .depth          = { 8 },
+                .memplanes      = 2,
+                .colplanes      = 1,
+                .mdataplanes    = 0x2, /* plane 1 holds frame meta data */
+                .mbus_code      = V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8,
+                .flags          = FMT_FLAGS_CAM | FMT_FLAGS_COMPRESSED,
         },
 };
 
@@ -371,7 +381,7 @@ int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
                 default:
                         return -EINVAL;
                 }
-        } else {
+        } else if (!frame->fmt->mdataplanes) {
                 if (frame->fmt->memplanes >= 2)
                         paddr->cb = vb2_dma_contig_plane_dma_addr(vb, 1);
 
@@ -698,6 +708,11 @@ int fimc_fill_format(struct fimc_frame *frame, struct v4l2_format *f)
                 if (frame->fmt->colplanes == 1) /* packed formats */
                         bpl = (bpl * frame->fmt->depth[0]) / 8;
                 pixm->plane_fmt[i].bytesperline = bpl;
+
+                if (frame->fmt->flags & FMT_FLAGS_COMPRESSED) {
+                        pixm->plane_fmt[i].sizeimage = frame->payload[i];
+                        continue;
+                }
                 pixm->plane_fmt[i].sizeimage = (frame->o_width *
                         frame->o_height * frame->fmt->depth[i]) / 8;
         }
diff --git a/drivers/media/platform/s5p-fimc/fimc-core.h b/drivers/media/platform/s5p-fimc/fimc-core.h
index cd716ba6015f..c0040d792499 100644
--- a/drivers/media/platform/s5p-fimc/fimc-core.h
+++ b/drivers/media/platform/s5p-fimc/fimc-core.h
@@ -40,6 +40,8 @@
 #define SCALER_MAX_VRATIO       64
 #define DMA_MIN_SIZE            8
 #define FIMC_CAMIF_MAX_HEIGHT   0x2000
+#define FIMC_MAX_JPEG_BUF_SIZE  (10 * SZ_1M)
+#define FIMC_MAX_PLANES         3
 
 /* indices to the clocks array */
 enum {
@@ -83,7 +85,7 @@ enum fimc_datapath {
 };
 
 enum fimc_color_fmt {
-        FIMC_FMT_RGB444 = 0x10,
+        FIMC_FMT_RGB444 = 0x10,
         FIMC_FMT_RGB555,
         FIMC_FMT_RGB565,
         FIMC_FMT_RGB666,
@@ -95,14 +97,15 @@ enum fimc_color_fmt {
         FIMC_FMT_CBYCRY422,
         FIMC_FMT_CRYCBY422,
         FIMC_FMT_YCBCR444_LOCAL,
-        FIMC_FMT_JPEG = 0x40,
-        FIMC_FMT_RAW8 = 0x80,
+        FIMC_FMT_RAW8 = 0x40,
         FIMC_FMT_RAW10,
         FIMC_FMT_RAW12,
+        FIMC_FMT_JPEG = 0x80,
+        FIMC_FMT_YUYV_JPEG = 0x100,
 };
 
+#define fimc_fmt_is_user_defined(x) (!!((x) & 0x180))
 #define fimc_fmt_is_rgb(x) (!!((x) & 0x10))
-#define fimc_fmt_is_jpeg(x) (!!((x) & 0x40))
 
 #define IS_M2M(__strt) ((__strt) == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE || \
                         __strt == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
@@ -139,6 +142,7 @@ enum fimc_color_fmt {
  * @memplanes: number of physically non-contiguous data planes
  * @colplanes: number of physically contiguous data planes
  * @depth: per plane driver's private 'number of bits per pixel'
+ * @mdataplanes: bitmask indicating meta data plane(s), (1 << plane_no)
  * @flags: flags indicating which operation mode format applies to
  */
 struct fimc_fmt {
@@ -149,12 +153,14 @@ struct fimc_fmt {
         u16     memplanes;
         u16     colplanes;
         u8      depth[VIDEO_MAX_PLANES];
+        u16     mdataplanes;
         u16     flags;
 #define FMT_FLAGS_CAM           (1 << 0)
 #define FMT_FLAGS_M2M_IN        (1 << 1)
 #define FMT_FLAGS_M2M_OUT       (1 << 2)
 #define FMT_FLAGS_M2M           (1 << 1 | 1 << 2)
 #define FMT_HAS_ALPHA           (1 << 3)
+#define FMT_FLAGS_COMPRESSED    (1 << 4)
 };
 
 /**
@@ -272,7 +278,7 @@ struct fimc_frame {
         u32     offs_v;
         u32     width;
         u32     height;
-        unsigned long           payload[VIDEO_MAX_PLANES];
+        unsigned int            payload[VIDEO_MAX_PLANES];
         struct fimc_addr        paddr;
         struct fimc_dma_offset  dma_offset;
         struct fimc_fmt         *fmt;
@@ -577,6 +583,18 @@ static inline int tiled_fmt(struct fimc_fmt *fmt)
         return fmt->fourcc == V4L2_PIX_FMT_NV12MT;
 }
 
+static inline bool fimc_jpeg_fourcc(u32 pixelformat)
+{
+        return (pixelformat == V4L2_PIX_FMT_JPEG ||
+                pixelformat == V4L2_PIX_FMT_S5C_UYVY_JPG);
+}
+
+static inline bool fimc_user_defined_mbus_fmt(u32 code)
+{
+        return (code == V4L2_MBUS_FMT_JPEG_1X8 ||
+                code == V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8);
+}
+
 /* Return the alpha component bit mask */
 static inline int fimc_get_alpha_mask(struct fimc_fmt *fmt)
 {
diff --git a/drivers/media/platform/s5p-fimc/fimc-m2m.c b/drivers/media/platform/s5p-fimc/fimc-m2m.c
index 6b71d953fd15..4500e44f6857 100644
--- a/drivers/media/platform/s5p-fimc/fimc-m2m.c
+++ b/drivers/media/platform/s5p-fimc/fimc-m2m.c
@@ -551,30 +551,31 @@ static int fimc_m2m_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
         return 0;
 }
 
-static int fimc_m2m_s_crop(struct file *file, void *fh, const struct v4l2_crop *cr)
+static int fimc_m2m_s_crop(struct file *file, void *fh, const struct v4l2_crop *crop)
 {
         struct fimc_ctx *ctx = fh_to_ctx(fh);
         struct fimc_dev *fimc = ctx->fimc_dev;
+        struct v4l2_crop cr = *crop;
         struct fimc_frame *f;
         int ret;
 
-        ret = fimc_m2m_try_crop(ctx, cr);
+        ret = fimc_m2m_try_crop(ctx, &cr);
         if (ret)
                 return ret;
 
-        f = (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
+        f = (cr.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
                 &ctx->s_frame : &ctx->d_frame;
 
         /* Check to see if scaling ratio is within supported range */
         if (fimc_ctx_state_is_set(FIMC_DST_FMT | FIMC_SRC_FMT, ctx)) {
-                if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-                        ret = fimc_check_scaler_ratio(ctx, cr->c.width,
-                                        cr->c.height, ctx->d_frame.width,
+                if (cr.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                        ret = fimc_check_scaler_ratio(ctx, cr.c.width,
+                                        cr.c.height, ctx->d_frame.width,
                                         ctx->d_frame.height, ctx->rotation);
                 } else {
                         ret = fimc_check_scaler_ratio(ctx, ctx->s_frame.width,
-                                        ctx->s_frame.height, cr->c.width,
-                                        cr->c.height, ctx->rotation);
+                                        ctx->s_frame.height, cr.c.width,
+                                        cr.c.height, ctx->rotation);
                 }
                 if (ret) {
                         v4l2_err(&fimc->m2m.vfd, "Out of scaler range\n");
@@ -582,10 +583,10 @@ static int fimc_m2m_s_crop(struct file *file, void *fh, const struct v4l2_crop *
                 }
         }
 
-        f->offs_h = cr->c.left;
-        f->offs_v = cr->c.top;
-        f->width  = cr->c.width;
-        f->height = cr->c.height;
+        f->offs_h = cr.c.left;
+        f->offs_v = cr.c.top;
+        f->width  = cr.c.width;
+        f->height = cr.c.height;
 
         fimc_ctx_state_set(FIMC_PARAMS, ctx);
 
diff --git a/drivers/media/platform/s5p-fimc/fimc-reg.c b/drivers/media/platform/s5p-fimc/fimc-reg.c
index 783408fd7d56..2c9d0c06c9e8 100644
--- a/drivers/media/platform/s5p-fimc/fimc-reg.c
+++ b/drivers/media/platform/s5p-fimc/fimc-reg.c
@@ -625,7 +625,7 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
                                 cfg |= FIMC_REG_CISRCFMT_ITU601_16BIT;
                 } /* else defaults to ITU-R BT.656 8-bit */
         } else if (cam->bus_type == FIMC_MIPI_CSI2) {
-                if (fimc_fmt_is_jpeg(f->fmt->color))
+                if (fimc_fmt_is_user_defined(f->fmt->color))
                         cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
         }
 
@@ -680,6 +680,7 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
                         tmp = FIMC_REG_CSIIMGFMT_YCBCR422_8BIT;
                         break;
                 case V4L2_MBUS_FMT_JPEG_1X8:
+                case V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8:
                         tmp = FIMC_REG_CSIIMGFMT_USER(1);
                         cfg |= FIMC_REG_CIGCTRL_CAM_JPEG;
                         break;
@@ -744,13 +745,13 @@ void fimc_hw_dis_capture(struct fimc_dev *dev)
 }
 
 /* Return an index to the buffer actually being written. */
-u32 fimc_hw_get_frame_index(struct fimc_dev *dev)
+s32 fimc_hw_get_frame_index(struct fimc_dev *dev)
 {
-        u32 reg;
+        s32 reg;
 
         if (dev->variant->has_cistatus2) {
-                reg = readl(dev->regs + FIMC_REG_CISTATUS2) & 0x3F;
-                return reg > 0 ? --reg : reg;
+                reg = readl(dev->regs + FIMC_REG_CISTATUS2) & 0x3f;
+                return reg - 1;
         }
 
         reg = readl(dev->regs + FIMC_REG_CISTATUS);
@@ -759,6 +760,18 @@ u32 fimc_hw_get_frame_index(struct fimc_dev *dev)
                 FIMC_REG_CISTATUS_FRAMECNT_SHIFT;
 }
 
+/* Return an index to the buffer being written previously. */
+s32 fimc_hw_get_prev_frame_index(struct fimc_dev *dev)
+{
+        s32 reg;
+
+        if (!dev->variant->has_cistatus2)
+                return -1;
+
+        reg = readl(dev->regs + FIMC_REG_CISTATUS2);
+        return ((reg >> 7) & 0x3f) - 1;
+}
+
 /* Locking: the caller holds fimc->slock */
 void fimc_activate_capture(struct fimc_ctx *ctx)
 {
diff --git a/drivers/media/platform/s5p-fimc/fimc-reg.h b/drivers/media/platform/s5p-fimc/fimc-reg.h
index 579ac8ac03de..b6abfc7b72ac 100644
--- a/drivers/media/platform/s5p-fimc/fimc-reg.h
+++ b/drivers/media/platform/s5p-fimc/fimc-reg.h
@@ -307,7 +307,8 @@ void fimc_hw_clear_irq(struct fimc_dev *dev);
 void fimc_hw_enable_scaler(struct fimc_dev *dev, bool on);
 void fimc_hw_activate_input_dma(struct fimc_dev *dev, bool on);
 void fimc_hw_dis_capture(struct fimc_dev *dev);
-u32 fimc_hw_get_frame_index(struct fimc_dev *dev);
+s32 fimc_hw_get_frame_index(struct fimc_dev *dev);
+s32 fimc_hw_get_prev_frame_index(struct fimc_dev *dev);
 void fimc_activate_capture(struct fimc_ctx *ctx);
 void fimc_deactivate_capture(struct fimc_dev *fimc);
 
diff --git a/drivers/media/platform/s5p-fimc/mipi-csis.c b/drivers/media/platform/s5p-fimc/mipi-csis.c
index e92236ac5cfe..4c961b1b68e6 100644
--- a/drivers/media/platform/s5p-fimc/mipi-csis.c
+++ b/drivers/media/platform/s5p-fimc/mipi-csis.c
@@ -2,7 +2,7 @@
  * Samsung S5P/EXYNOS4 SoC series MIPI-CSI receiver driver
  *
  * Copyright (C) 2011 - 2012 Samsung Electronics Co., Ltd.
- * Sylwester Nawrocki, <s.nawrocki@samsung.com>
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -98,6 +98,11 @@ MODULE_PARM_DESC(debug, "Debug level (0-2)");
 #define CSIS_MAX_PIX_WIDTH              0xffff
 #define CSIS_MAX_PIX_HEIGHT             0xffff
 
+/* Non-image packet data buffers */
+#define S5PCSIS_PKTDATA_ODD             0x2000
+#define S5PCSIS_PKTDATA_EVEN            0x3000
+#define S5PCSIS_PKTDATA_SIZE            SZ_4K
+
 enum {
         CSIS_CLK_MUX,
         CSIS_CLK_GATE,
@@ -110,8 +115,8 @@ static char *csi_clock_name[] = {
 #define NUM_CSIS_CLOCKS ARRAY_SIZE(csi_clock_name)
 
 static const char * const csis_supply_name[] = {
-        "vdd11", /* 1.1V or 1.2V (s5pc100) MIPI CSI suppply */
-        "vdd18", /* VDD 1.8V and MIPI CSI PLL supply */
+        "vddcore",  /* CSIS Core (1.0V, 1.1V or 1.2V) suppply */
+        "vddio",    /* CSIS I/O and PLL (1.8V) supply */
 };
 #define CSIS_NUM_SUPPLIES ARRAY_SIZE(csis_supply_name)
 
@@ -144,12 +149,18 @@ static const struct s5pcsis_event s5pcsis_events[] = {
 };
 #define S5PCSIS_NUM_EVENTS ARRAY_SIZE(s5pcsis_events)
 
+struct csis_pktbuf {
+        u32 *data;
+        unsigned int len;
+};
+
 /**
  * struct csis_state - the driver's internal state data structure
  * @lock: mutex serializing the subdev and power management operations,
  *        protecting @format and @flags members
  * @pads: CSIS pads array
  * @sd: v4l2_subdev associated with CSIS device instance
+ * @index: the hardware instance index
  * @pdev: CSIS platform device
  * @regs: mmaped I/O registers memory
  * @supplies: CSIS regulator supplies
@@ -159,12 +170,14 @@ static const struct s5pcsis_event s5pcsis_events[] = {
  * @csis_fmt: current CSIS pixel format
  * @format: common media bus format for the source and sink pad
  * @slock: spinlock protecting structure members below
+ * @pkt_buf: the frame embedded (non-image) data buffer
  * @events: MIPI-CSIS event (error) counters
  */
 struct csis_state {
         struct mutex lock;
         struct media_pad pads[CSIS_PADS_NUM];
         struct v4l2_subdev sd;
+        u8 index;
         struct platform_device *pdev;
         void __iomem *regs;
         struct regulator_bulk_data supplies[CSIS_NUM_SUPPLIES];
@@ -175,6 +188,7 @@ struct csis_state {
         struct v4l2_mbus_framefmt format;
 
         struct spinlock slock;
+        struct csis_pktbuf pkt_buf;
         struct s5pcsis_event events[S5PCSIS_NUM_EVENTS];
 };
 
@@ -202,7 +216,11 @@ static const struct csis_pix_format s5pcsis_formats[] = {
                 .code = V4L2_MBUS_FMT_JPEG_1X8,
                 .fmt_reg = S5PCSIS_CFG_FMT_USER(1),
                 .data_alignment = 32,
-        },
+        }, {
+                .code = V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8,
+                .fmt_reg = S5PCSIS_CFG_FMT_USER(1),
+                .data_alignment = 32,
+        }
 };
 
 #define s5pcsis_write(__csis, __r, __v) writel(__v, __csis->regs + __r)
@@ -266,7 +284,7 @@ static void __s5pcsis_set_format(struct csis_state *state)
         struct v4l2_mbus_framefmt *mf = &state->format;
         u32 val;
 
-        v4l2_dbg(1, debug, &state->sd, "fmt: %d, %d x %d\n",
+        v4l2_dbg(1, debug, &state->sd, "fmt: %#x, %d x %d\n",
                  mf->code, mf->width, mf->height);
 
         /* Color format */
@@ -304,8 +322,10 @@ static void s5pcsis_set_params(struct csis_state *state)
                 val |= S5PCSIS_CTRL_ALIGN_32BIT;
         else /* 24-bits */
                 val &= ~S5PCSIS_CTRL_ALIGN_32BIT;
-        /* Not using external clock. */
+
         val &= ~S5PCSIS_CTRL_WCLK_EXTCLK;
+        if (pdata->wclk_source)
+                val |= S5PCSIS_CTRL_WCLK_EXTCLK;
         s5pcsis_write(state, S5PCSIS_CTRL, val);
 
         /* Update the shadow register. */
@@ -529,6 +549,22 @@ static int s5pcsis_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
         return 0;
 }
 
+static int s5pcsis_s_rx_buffer(struct v4l2_subdev *sd, void *buf,
+                               unsigned int *size)
+{
+        struct csis_state *state = sd_to_csis_state(sd);
+        unsigned long flags;
+
+        *size = min_t(unsigned int, *size, S5PCSIS_PKTDATA_SIZE);
+
+        spin_lock_irqsave(&state->slock, flags);
+        state->pkt_buf.data = buf;
+        state->pkt_buf.len = *size;
+        spin_unlock_irqrestore(&state->slock, flags);
+
+        return 0;
+}
+
 static int s5pcsis_log_status(struct v4l2_subdev *sd)
 {
         struct csis_state *state = sd_to_csis_state(sd);
@@ -566,6 +602,7 @@ static struct v4l2_subdev_pad_ops s5pcsis_pad_ops = {
 };
 
 static struct v4l2_subdev_video_ops s5pcsis_video_ops = {
+        .s_rx_buffer = s5pcsis_s_rx_buffer,
         .s_stream = s5pcsis_s_stream,
 };
 
@@ -578,13 +615,26 @@ static struct v4l2_subdev_ops s5pcsis_subdev_ops = {
 static irqreturn_t s5pcsis_irq_handler(int irq, void *dev_id)
 {
         struct csis_state *state = dev_id;
+        struct csis_pktbuf *pktbuf = &state->pkt_buf;
         unsigned long flags;
         u32 status;
 
         status = s5pcsis_read(state, S5PCSIS_INTSRC);
-
         spin_lock_irqsave(&state->slock, flags);
 
+        if ((status & S5PCSIS_INTSRC_NON_IMAGE_DATA) && pktbuf->data) {
+                u32 offset;
+
+                if (status & S5PCSIS_INTSRC_EVEN)
+                        offset = S5PCSIS_PKTDATA_EVEN;
+                else
+                        offset = S5PCSIS_PKTDATA_ODD;
+
+                memcpy(pktbuf->data, state->regs + offset, pktbuf->len);
+                pktbuf->data = NULL;
+                rmb();
+        }
+
         /* Update the event/error counters */
         if ((status & S5PCSIS_INTSRC_ERRORS) || debug) {
                 int i;
@@ -620,14 +670,15 @@ static int __devinit s5pcsis_probe(struct platform_device *pdev)
         spin_lock_init(&state->slock);
 
         state->pdev = pdev;
+        state->index = max(0, pdev->id);
 
         pdata = pdev->dev.platform_data;
-        if (pdata == NULL || pdata->phy_enable == NULL) {
+        if (pdata == NULL) {
                 dev_err(&pdev->dev, "Platform data not fully specified\n");
                 return -EINVAL;
         }
 
-        if ((pdev->id == 1 && pdata->lanes > CSIS1_MAX_LANES) ||
+        if ((state->index == 1 && pdata->lanes > CSIS1_MAX_LANES) ||
             pdata->lanes > CSIS0_MAX_LANES) {
                 dev_err(&pdev->dev, "Unsupported number of data lanes: %d\n",
                         pdata->lanes);
@@ -710,7 +761,6 @@ e_clkput:
 
 static int s5pcsis_pm_suspend(struct device *dev, bool runtime)
 {
-        struct s5p_platform_mipi_csis *pdata = dev->platform_data;
         struct platform_device *pdev = to_platform_device(dev);
         struct v4l2_subdev *sd = platform_get_drvdata(pdev);
         struct csis_state *state = sd_to_csis_state(sd);
@@ -722,7 +772,7 @@ static int s5pcsis_pm_suspend(struct device *dev, bool runtime)
         mutex_lock(&state->lock);
         if (state->flags & ST_POWERED) {
                 s5pcsis_stop_stream(state);
-                ret = pdata->phy_enable(state->pdev, false);
+                ret = s5p_csis_phy_enable(state->index, false);
                 if (ret)
                         goto unlock;
                 ret = regulator_bulk_disable(CSIS_NUM_SUPPLIES,
@@ -741,7 +791,6 @@ static int s5pcsis_pm_suspend(struct device *dev, bool runtime)
 
 static int s5pcsis_pm_resume(struct device *dev, bool runtime)
 {
-        struct s5p_platform_mipi_csis *pdata = dev->platform_data;
         struct platform_device *pdev = to_platform_device(dev);
         struct v4l2_subdev *sd = platform_get_drvdata(pdev);
         struct csis_state *state = sd_to_csis_state(sd);
@@ -759,7 +808,7 @@ static int s5pcsis_pm_resume(struct device *dev, bool runtime)
                                             state->supplies);
                 if (ret)
                         goto unlock;
-                ret = pdata->phy_enable(state->pdev, true);
+                ret = s5p_csis_phy_enable(state->index, true);
                 if (!ret) {
                         state->flags |= ST_POWERED;
                 } else {
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
index 1e3b9dd014c0..1bfbc325836b 100644
--- a/drivers/media/platform/s5p-g2d/g2d.c
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -507,7 +507,7 @@ static int vidioc_g_crop(struct file *file, void *prv, struct v4l2_crop *cr)
         return 0;
 }
 
-static int vidioc_try_crop(struct file *file, void *prv, struct v4l2_crop *cr)
+static int vidioc_try_crop(struct file *file, void *prv, const struct v4l2_crop *cr)
 {
         struct g2d_ctx *ctx = prv;
         struct g2d_dev *dev = ctx->dev;
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index 394775ae5774..17983c4c9a9a 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -1353,7 +1353,7 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
                 return ret;
         }
         dev_dbg(&pdev->dev, "clock source %p\n", jpeg->clk);
-        clk_enable(jpeg->clk);
+        clk_prepare_enable(jpeg->clk);
 
         /* v4l2 device */
         ret = v4l2_device_register(&pdev->dev, &jpeg->v4l2_dev);
@@ -1460,7 +1460,7 @@ device_register_rollback:
         v4l2_device_unregister(&jpeg->v4l2_dev);
 
 clk_get_rollback:
-        clk_disable(jpeg->clk);
+        clk_disable_unprepare(jpeg->clk);
         clk_put(jpeg->clk);
 
         return ret;
@@ -1480,7 +1480,7 @@ static int s5p_jpeg_remove(struct platform_device *pdev)
         v4l2_m2m_release(jpeg->m2m_dev);
         v4l2_device_unregister(&jpeg->v4l2_dev);
 
-        clk_disable(jpeg->clk);
+        clk_disable_unprepare(jpeg->clk);
         clk_put(jpeg->clk);
 
         return 0;
diff --git a/drivers/media/platform/s5p-mfc/Makefile b/drivers/media/platform/s5p-mfc/Makefile
index d0663409af00..379008c6d09a 100644
--- a/drivers/media/platform/s5p-mfc/Makefile
+++ b/drivers/media/platform/s5p-mfc/Makefile
@@ -1,5 +1,6 @@
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MFC) := s5p-mfc.o
-s5p-mfc-y += s5p_mfc.o s5p_mfc_intr.o s5p_mfc_opr.o
+s5p-mfc-y += s5p_mfc.o s5p_mfc_intr.o
 s5p-mfc-y += s5p_mfc_dec.o s5p_mfc_enc.o
-s5p-mfc-y += s5p_mfc_ctrl.o s5p_mfc_cmd.o
-s5p-mfc-y += s5p_mfc_pm.o s5p_mfc_shm.o
+s5p-mfc-y += s5p_mfc_ctrl.o s5p_mfc_pm.o
+s5p-mfc-y += s5p_mfc_opr.o s5p_mfc_opr_v5.o s5p_mfc_opr_v6.o
+s5p-mfc-y += s5p_mfc_cmd.o s5p_mfc_cmd_v5.o s5p_mfc_cmd_v6.o
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc-v6.h b/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
new file mode 100644
index 000000000000..363a97cc7681
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
@@ -0,0 +1,408 @@
+/*
+ * Register definition file for Samsung MFC V6.x Interface (FIMV) driver
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _REGS_FIMV_V6_H
+#define _REGS_FIMV_V6_H
+
+#include <linux/kernel.h>
+#include <linux/sizes.h>
+
+#define S5P_FIMV_REG_SIZE_V6    (S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR)
+#define S5P_FIMV_REG_COUNT_V6   ((S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR) / 4)
+
+/* Number of bits that the buffer address should be shifted for particular
+ * MFC buffers.  */
+#define S5P_FIMV_MEM_OFFSET_V6          0
+
+#define S5P_FIMV_START_ADDR_V6          0x0000
+#define S5P_FIMV_END_ADDR_V6            0xfd80
+
+#define S5P_FIMV_REG_CLEAR_BEGIN_V6     0xf000
+#define S5P_FIMV_REG_CLEAR_COUNT_V6     1024
+
+/* Codec Common Registers */
+#define S5P_FIMV_RISC_ON_V6                     0x0000
+#define S5P_FIMV_RISC2HOST_INT_V6               0x003C
+#define S5P_FIMV_HOST2RISC_INT_V6               0x0044
+#define S5P_FIMV_RISC_BASE_ADDRESS_V6           0x0054
+
+#define S5P_FIMV_MFC_RESET_V6                   0x1070
+
+#define S5P_FIMV_HOST2RISC_CMD_V6               0x1100
+#define S5P_FIMV_H2R_CMD_EMPTY_V6               0
+#define S5P_FIMV_H2R_CMD_SYS_INIT_V6            1
+#define S5P_FIMV_H2R_CMD_OPEN_INSTANCE_V6       2
+#define S5P_FIMV_CH_SEQ_HEADER_V6               3
+#define S5P_FIMV_CH_INIT_BUFS_V6                4
+#define S5P_FIMV_CH_FRAME_START_V6              5
+#define S5P_FIMV_H2R_CMD_CLOSE_INSTANCE_V6      6
+#define S5P_FIMV_H2R_CMD_SLEEP_V6               7
+#define S5P_FIMV_H2R_CMD_WAKEUP_V6              8
+#define S5P_FIMV_CH_LAST_FRAME_V6               9
+#define S5P_FIMV_H2R_CMD_FLUSH_V6               10
+/* RMVME: REALLOC used? */
+#define S5P_FIMV_CH_FRAME_START_REALLOC_V6      5
+
+#define S5P_FIMV_RISC2HOST_CMD_V6               0x1104
+#define S5P_FIMV_R2H_CMD_EMPTY_V6               0
+#define S5P_FIMV_R2H_CMD_SYS_INIT_RET_V6        1
+#define S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET_V6   2
+#define S5P_FIMV_R2H_CMD_SEQ_DONE_RET_V6        3
+#define S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET_V6    4
+
+#define S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET_V6  6
+#define S5P_FIMV_R2H_CMD_SLEEP_RET_V6           7
+#define S5P_FIMV_R2H_CMD_WAKEUP_RET_V6          8
+#define S5P_FIMV_R2H_CMD_COMPLETE_SEQ_RET_V6    9
+#define S5P_FIMV_R2H_CMD_DPB_FLUSH_RET_V6       10
+#define S5P_FIMV_R2H_CMD_NAL_ABORT_RET_V6       11
+#define S5P_FIMV_R2H_CMD_FW_STATUS_RET_V6       12
+#define S5P_FIMV_R2H_CMD_FRAME_DONE_RET_V6      13
+#define S5P_FIMV_R2H_CMD_FIELD_DONE_RET_V6      14
+#define S5P_FIMV_R2H_CMD_SLICE_DONE_RET_V6      15
+#define S5P_FIMV_R2H_CMD_ENC_BUFFER_FUL_RET_V6  16
+#define S5P_FIMV_R2H_CMD_ERR_RET_V6             32
+
+#define S5P_FIMV_FW_VERSION_V6                  0xf000
+
+#define S5P_FIMV_INSTANCE_ID_V6                 0xf008
+#define S5P_FIMV_CODEC_TYPE_V6                  0xf00c
+#define S5P_FIMV_CONTEXT_MEM_ADDR_V6            0xf014
+#define S5P_FIMV_CONTEXT_MEM_SIZE_V6            0xf018
+#define S5P_FIMV_PIXEL_FORMAT_V6                0xf020
+
+#define S5P_FIMV_METADATA_ENABLE_V6             0xf024
+#define S5P_FIMV_DBG_BUFFER_ADDR_V6             0xf030
+#define S5P_FIMV_DBG_BUFFER_SIZE_V6             0xf034
+#define S5P_FIMV_RET_INSTANCE_ID_V6             0xf070
+
+#define S5P_FIMV_ERROR_CODE_V6                  0xf074
+#define S5P_FIMV_ERR_WARNINGS_START_V6          160
+#define S5P_FIMV_ERR_DEC_MASK_V6                0xffff
+#define S5P_FIMV_ERR_DEC_SHIFT_V6               0
+#define S5P_FIMV_ERR_DSPL_MASK_V6               0xffff0000
+#define S5P_FIMV_ERR_DSPL_SHIFT_V6              16
+
+#define S5P_FIMV_DBG_BUFFER_OUTPUT_SIZE_V6      0xf078
+#define S5P_FIMV_METADATA_STATUS_V6             0xf07C
+#define S5P_FIMV_METADATA_ADDR_MB_INFO_V6       0xf080
+#define S5P_FIMV_METADATA_SIZE_MB_INFO_V6       0xf084
+
+/* Decoder Registers */
+#define S5P_FIMV_D_CRC_CTRL_V6                  0xf0b0
+#define S5P_FIMV_D_DEC_OPTIONS_V6               0xf0b4
+#define S5P_FIMV_D_OPT_FMO_ASO_CTRL_MASK_V6     4
+#define S5P_FIMV_D_OPT_DDELAY_EN_SHIFT_V6       3
+#define S5P_FIMV_D_OPT_LF_CTRL_SHIFT_V6         1
+#define S5P_FIMV_D_OPT_LF_CTRL_MASK_V6          0x3
+#define S5P_FIMV_D_OPT_TILE_MODE_SHIFT_V6       0
+
+#define S5P_FIMV_D_DISPLAY_DELAY_V6             0xf0b8
+
+#define S5P_FIMV_D_SET_FRAME_WIDTH_V6           0xf0bc
+#define S5P_FIMV_D_SET_FRAME_HEIGHT_V6          0xf0c0
+
+#define S5P_FIMV_D_SEI_ENABLE_V6                0xf0c4
+
+/* Buffer setting registers */
+#define S5P_FIMV_D_MIN_NUM_DPB_V6               0xf0f0
+#define S5P_FIMV_D_MIN_LUMA_DPB_SIZE_V6         0xf0f4
+#define S5P_FIMV_D_MIN_CHROMA_DPB_SIZE_V6       0xf0f8
+#define S5P_FIMV_D_MVC_NUM_VIEWS_V6             0xf0fc
+#define S5P_FIMV_D_MIN_NUM_MV_V6                0xf100
+#define S5P_FIMV_D_NUM_DPB_V6                   0xf130
+#define S5P_FIMV_D_LUMA_DPB_SIZE_V6             0xf134
+#define S5P_FIMV_D_CHROMA_DPB_SIZE_V6           0xf138
+#define S5P_FIMV_D_MV_BUFFER_SIZE_V6            0xf13c
+
+#define S5P_FIMV_D_LUMA_DPB_V6                  0xf140
+#define S5P_FIMV_D_CHROMA_DPB_V6                0xf240
+#define S5P_FIMV_D_MV_BUFFER_V6                 0xf340
+
+#define S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V6       0xf440
+#define S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V6       0xf444
+#define S5P_FIMV_D_METADATA_BUFFER_ADDR_V6      0xf448
+#define S5P_FIMV_D_METADATA_BUFFER_SIZE_V6      0xf44c
+#define S5P_FIMV_D_NUM_MV_V6                    0xf478
+#define S5P_FIMV_D_CPB_BUFFER_ADDR_V6           0xf4b0
+#define S5P_FIMV_D_CPB_BUFFER_SIZE_V6           0xf4b4
+
+#define S5P_FIMV_D_AVAILABLE_DPB_FLAG_UPPER_V6  0xf4b8
+#define S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V6  0xf4bc
+#define S5P_FIMV_D_CPB_BUFFER_OFFSET_V6         0xf4c0
+#define S5P_FIMV_D_SLICE_IF_ENABLE_V6           0xf4c4
+#define S5P_FIMV_D_PICTURE_TAG_V6               0xf4c8
+#define S5P_FIMV_D_STREAM_DATA_SIZE_V6          0xf4d0
+
+/* Display information register */
+#define S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V6       0xf500
+#define S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V6      0xf504
+
+/* Display status */
+#define S5P_FIMV_D_DISPLAY_STATUS_V6            0xf508
+
+#define S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6         0xf50c
+#define S5P_FIMV_D_DISPLAY_CHROMA_ADDR_V6       0xf510
+
+#define S5P_FIMV_D_DISPLAY_FRAME_TYPE_V6        0xf514
+
+#define S5P_FIMV_D_DISPLAY_CROP_INFO1_V6        0xf518
+#define S5P_FIMV_D_DISPLAY_CROP_INFO2_V6        0xf51c
+#define S5P_FIMV_D_DISPLAY_PICTURE_PROFILE_V6   0xf520
+#define S5P_FIMV_D_DISPLAY_LUMA_CRC_TOP_V6      0xf524
+#define S5P_FIMV_D_DISPLAY_CHROMA_CRC_TOP_V6    0xf528
+#define S5P_FIMV_D_DISPLAY_LUMA_CRC_BOT_V6      0xf52c
+#define S5P_FIMV_D_DISPLAY_CHROMA_CRC_BOT_V6    0xf530
+#define S5P_FIMV_D_DISPLAY_ASPECT_RATIO_V6      0xf534
+#define S5P_FIMV_D_DISPLAY_EXTENDED_AR_V6       0xf538
+
+/* Decoded picture information register */
+#define S5P_FIMV_D_DECODED_FRAME_WIDTH_V6       0xf53c
+#define S5P_FIMV_D_DECODED_FRAME_HEIGHT_V6      0xf540
+#define S5P_FIMV_D_DECODED_STATUS_V6            0xf544
+#define S5P_FIMV_DEC_CRC_GEN_MASK_V6            0x1
+#define S5P_FIMV_DEC_CRC_GEN_SHIFT_V6           6
+
+#define S5P_FIMV_D_DECODED_LUMA_ADDR_V6         0xf548
+#define S5P_FIMV_D_DECODED_CHROMA_ADDR_V6       0xf54c
+
+#define S5P_FIMV_D_DECODED_FRAME_TYPE_V6        0xf550
+#define S5P_FIMV_DECODE_FRAME_MASK_V6           7
+
+#define S5P_FIMV_D_DECODED_CROP_INFO1_V6        0xf554
+#define S5P_FIMV_D_DECODED_CROP_INFO2_V6        0xf558
+#define S5P_FIMV_D_DECODED_PICTURE_PROFILE_V6   0xf55c
+#define S5P_FIMV_D_DECODED_NAL_SIZE_V6          0xf560
+#define S5P_FIMV_D_DECODED_LUMA_CRC_TOP_V6      0xf564
+#define S5P_FIMV_D_DECODED_CHROMA_CRC_TOP_V6    0xf568
+#define S5P_FIMV_D_DECODED_LUMA_CRC_BOT_V6      0xf56c
+#define S5P_FIMV_D_DECODED_CHROMA_CRC_BOT_V6    0xf570
+
+/* Returned value register for specific setting */
+#define S5P_FIMV_D_RET_PICTURE_TAG_TOP_V6               0xf574
+#define S5P_FIMV_D_RET_PICTURE_TAG_BOT_V6               0xf578
+#define S5P_FIMV_D_RET_PICTURE_TIME_TOP_V6              0xf57c
+#define S5P_FIMV_D_RET_PICTURE_TIME_BOT_V6              0xf580
+#define S5P_FIMV_D_CHROMA_FORMAT_V6                     0xf588
+#define S5P_FIMV_D_MPEG4_INFO_V6                        0xf58c
+#define S5P_FIMV_D_H264_INFO_V6                         0xf590
+
+#define S5P_FIMV_D_METADATA_ADDR_CONCEALED_MB_V6        0xf594
+#define S5P_FIMV_D_METADATA_SIZE_CONCEALED_MB_V6        0xf598
+#define S5P_FIMV_D_METADATA_ADDR_VC1_PARAM_V6           0xf59c
+#define S5P_FIMV_D_METADATA_SIZE_VC1_PARAM_V6           0xf5a0
+#define S5P_FIMV_D_METADATA_ADDR_SEI_NAL_V6             0xf5a4
+#define S5P_FIMV_D_METADATA_SIZE_SEI_NAL_V6             0xf5a8
+#define S5P_FIMV_D_METADATA_ADDR_VUI_V6                 0xf5ac
+#define S5P_FIMV_D_METADATA_SIZE_VUI_V6                 0xf5b0
+
+#define S5P_FIMV_D_MVC_VIEW_ID_V6               0xf5b4
+
+/* SEI related information */
+#define S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V6      0xf5f0
+#define S5P_FIMV_D_FRAME_PACK_ARRGMENT_ID_V6    0xf5f4
+#define S5P_FIMV_D_FRAME_PACK_SEI_INFO_V6       0xf5f8
+#define S5P_FIMV_D_FRAME_PACK_GRID_POS_V6       0xf5fc
+
+/* Encoder Registers */
+#define S5P_FIMV_E_FRAME_WIDTH_V6               0xf770
+#define S5P_FIMV_E_FRAME_HEIGHT_V6              0xf774
+#define S5P_FIMV_E_CROPPED_FRAME_WIDTH_V6       0xf778
+#define S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6      0xf77c
+#define S5P_FIMV_E_FRAME_CROP_OFFSET_V6         0xf780
+#define S5P_FIMV_E_ENC_OPTIONS_V6               0xf784
+#define S5P_FIMV_E_PICTURE_PROFILE_V6           0xf788
+#define S5P_FIMV_E_FIXED_PICTURE_QP_V6          0xf790
+
+#define S5P_FIMV_E_RC_CONFIG_V6                 0xf794
+#define S5P_FIMV_E_RC_QP_BOUND_V6               0xf798
+#define S5P_FIMV_E_RC_RPARAM_V6                 0xf79c
+#define S5P_FIMV_E_MB_RC_CONFIG_V6              0xf7a0
+#define S5P_FIMV_E_PADDING_CTRL_V6              0xf7a4
+#define S5P_FIMV_E_MV_HOR_RANGE_V6              0xf7ac
+#define S5P_FIMV_E_MV_VER_RANGE_V6              0xf7b0
+
+#define S5P_FIMV_E_VBV_BUFFER_SIZE_V6           0xf84c
+#define S5P_FIMV_E_VBV_INIT_DELAY_V6            0xf850
+#define S5P_FIMV_E_NUM_DPB_V6                   0xf890
+#define S5P_FIMV_E_LUMA_DPB_V6                  0xf8c0
+#define S5P_FIMV_E_CHROMA_DPB_V6                0xf904
+#define S5P_FIMV_E_ME_BUFFER_V6                 0xf948
+
+#define S5P_FIMV_E_SCRATCH_BUFFER_ADDR_V6       0xf98c
+#define S5P_FIMV_E_SCRATCH_BUFFER_SIZE_V6       0xf990
+#define S5P_FIMV_E_TMV_BUFFER0_V6               0xf994
+#define S5P_FIMV_E_TMV_BUFFER1_V6               0xf998
+#define S5P_FIMV_E_SOURCE_LUMA_ADDR_V6          0xf9f0
+#define S5P_FIMV_E_SOURCE_CHROMA_ADDR_V6        0xf9f4
+#define S5P_FIMV_E_STREAM_BUFFER_ADDR_V6        0xf9f8
+#define S5P_FIMV_E_STREAM_BUFFER_SIZE_V6        0xf9fc
+#define S5P_FIMV_E_ROI_BUFFER_ADDR_V6           0xfA00
+
+#define S5P_FIMV_E_PARAM_CHANGE_V6              0xfa04
+#define S5P_FIMV_E_IR_SIZE_V6                   0xfa08
+#define S5P_FIMV_E_GOP_CONFIG_V6                0xfa0c
+#define S5P_FIMV_E_MSLICE_MODE_V6               0xfa10
+#define S5P_FIMV_E_MSLICE_SIZE_MB_V6            0xfa14
+#define S5P_FIMV_E_MSLICE_SIZE_BITS_V6          0xfa18
+#define S5P_FIMV_E_FRAME_INSERTION_V6           0xfa1c
+
+#define S5P_FIMV_E_RC_FRAME_RATE_V6             0xfa20
+#define S5P_FIMV_E_RC_BIT_RATE_V6               0xfa24
+#define S5P_FIMV_E_RC_QP_OFFSET_V6              0xfa28
+#define S5P_FIMV_E_RC_ROI_CTRL_V6               0xfa2c
+#define S5P_FIMV_E_PICTURE_TAG_V6               0xfa30
+#define S5P_FIMV_E_BIT_COUNT_ENABLE_V6          0xfa34
+#define S5P_FIMV_E_MAX_BIT_COUNT_V6             0xfa38
+#define S5P_FIMV_E_MIN_BIT_COUNT_V6             0xfa3c
+
+#define S5P_FIMV_E_METADATA_BUFFER_ADDR_V6              0xfa40
+#define S5P_FIMV_E_METADATA_BUFFER_SIZE_V6              0xfa44
+#define S5P_FIMV_E_STREAM_SIZE_V6                       0xfa80
+#define S5P_FIMV_E_SLICE_TYPE_V6                        0xfa84
+#define S5P_FIMV_E_PICTURE_COUNT_V6                     0xfa88
+#define S5P_FIMV_E_RET_PICTURE_TAG_V6                   0xfa8c
+#define S5P_FIMV_E_STREAM_BUFFER_WRITE_POINTER_V6       0xfa90
+
+#define S5P_FIMV_E_ENCODED_SOURCE_LUMA_ADDR_V6          0xfa94
+#define S5P_FIMV_E_ENCODED_SOURCE_CHROMA_ADDR_V6        0xfa98
+#define S5P_FIMV_E_RECON_LUMA_DPB_ADDR_V6               0xfa9c
+#define S5P_FIMV_E_RECON_CHROMA_DPB_ADDR_V6             0xfaa0
+#define S5P_FIMV_E_METADATA_ADDR_ENC_SLICE_V6           0xfaa4
+#define S5P_FIMV_E_METADATA_SIZE_ENC_SLICE_V6           0xfaa8
+
+#define S5P_FIMV_E_MPEG4_OPTIONS_V6             0xfb10
+#define S5P_FIMV_E_MPEG4_HEC_PERIOD_V6          0xfb14
+#define S5P_FIMV_E_ASPECT_RATIO_V6              0xfb50
+#define S5P_FIMV_E_EXTENDED_SAR_V6              0xfb54
+
+#define S5P_FIMV_E_H264_OPTIONS_V6              0xfb58
+#define S5P_FIMV_E_H264_LF_ALPHA_OFFSET_V6      0xfb5c
+#define S5P_FIMV_E_H264_LF_BETA_OFFSET_V6       0xfb60
+#define S5P_FIMV_E_H264_I_PERIOD_V6             0xfb64
+
+#define S5P_FIMV_E_H264_FMO_SLICE_GRP_MAP_TYPE_V6               0xfb68
+#define S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6             0xfb6c
+#define S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_DIR_V6             0xfb70
+#define S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_RATE_MINUS1_V6     0xfb74
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_0_V6              0xfb78
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_1_V6              0xfb7c
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_2_V6              0xfb80
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_3_V6              0xfb84
+
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_0_V6    0xfb88
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_1_V6    0xfb8c
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_2_V6    0xfb90
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_3_V6    0xfb94
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_4_V6    0xfb98
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_5_V6    0xfb9c
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_6_V6    0xfba0
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_7_V6    0xfba4
+
+#define S5P_FIMV_E_H264_CHROMA_QP_OFFSET_V6     0xfba8
+#define S5P_FIMV_E_H264_NUM_T_LAYER_V6          0xfbac
+
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER0_V6       0xfbb0
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER1_V6       0xfbb4
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER2_V6       0xfbb8
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER3_V6       0xfbbc
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER4_V6       0xfbc0
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER5_V6       0xfbc4
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER6_V6       0xfbc8
+
+#define S5P_FIMV_E_H264_FRAME_PACKING_SEI_INFO_V6               0xfc4c
+#define S5P_FIMV_ENC_FP_ARRANGEMENT_TYPE_SIDE_BY_SIDE_V6        0
+#define S5P_FIMV_ENC_FP_ARRANGEMENT_TYPE_TOP_BOTTOM_V6          1
+#define S5P_FIMV_ENC_FP_ARRANGEMENT_TYPE_TEMPORAL_V6            2
+
+#define S5P_FIMV_E_MVC_FRAME_QP_VIEW1_V6                0xfd40
+#define S5P_FIMV_E_MVC_RC_FRAME_RATE_VIEW1_V6           0xfd44
+#define S5P_FIMV_E_MVC_RC_BIT_RATE_VIEW1_V6             0xfd48
+#define S5P_FIMV_E_MVC_RC_QBOUND_VIEW1_V6               0xfd4c
+#define S5P_FIMV_E_MVC_RC_RPARA_VIEW1_V6                0xfd50
+#define S5P_FIMV_E_MVC_INTER_VIEW_PREDICTION_ON_V6      0xfd80
+
+/* Codec numbers  */
+#define S5P_FIMV_CODEC_NONE_V6          -1
+
+
+#define S5P_FIMV_CODEC_H264_DEC_V6      0
+#define S5P_FIMV_CODEC_H264_MVC_DEC_V6  1
+
+#define S5P_FIMV_CODEC_MPEG4_DEC_V6     3
+#define S5P_FIMV_CODEC_FIMV1_DEC_V6     4
+#define S5P_FIMV_CODEC_FIMV2_DEC_V6     5
+#define S5P_FIMV_CODEC_FIMV3_DEC_V6     6
+#define S5P_FIMV_CODEC_FIMV4_DEC_V6     7
+#define S5P_FIMV_CODEC_H263_DEC_V6      8
+#define S5P_FIMV_CODEC_VC1RCV_DEC_V6    9
+#define S5P_FIMV_CODEC_VC1_DEC_V6       10
+/* FIXME: Add 11~12 */
+#define S5P_FIMV_CODEC_MPEG2_DEC_V6     13
+#define S5P_FIMV_CODEC_VP8_DEC_V6       14
+/* FIXME: Add 15~16 */
+#define S5P_FIMV_CODEC_H264_ENC_V6      20
+#define S5P_FIMV_CODEC_H264_MVC_ENC_V6  21
+
+#define S5P_FIMV_CODEC_MPEG4_ENC_V6     23
+#define S5P_FIMV_CODEC_H263_ENC_V6      24
+
+#define S5P_FIMV_NV12M_HALIGN_V6                16
+#define S5P_FIMV_NV12MT_HALIGN_V6               16
+#define S5P_FIMV_NV12MT_VALIGN_V6               16
+
+#define S5P_FIMV_TMV_BUFFER_ALIGN_V6            16
+#define S5P_FIMV_LUMA_DPB_BUFFER_ALIGN_V6       256
+#define S5P_FIMV_CHROMA_DPB_BUFFER_ALIGN_V6     256
+#define S5P_FIMV_ME_BUFFER_ALIGN_V6             256
+#define S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6        256
+
+#define S5P_FIMV_LUMA_MB_TO_PIXEL_V6            256
+#define S5P_FIMV_CHROMA_MB_TO_PIXEL_V6          128
+#define S5P_FIMV_NUM_TMV_BUFFERS_V6             2
+
+#define S5P_FIMV_MAX_FRAME_SIZE_V6              (2 * SZ_1M)
+#define S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6        16
+#define S5P_FIMV_NUM_PIXELS_IN_MB_COL_V6        16
+
+/* Buffer size requirements defined by hardware */
+#define S5P_FIMV_TMV_BUFFER_SIZE_V6(w, h)       (((w) + 1) * ((h) + 1) * 8)
+#define S5P_FIMV_ME_BUFFER_SIZE_V6(imw, imh, mbw, mbh) \
+        ((DIV_ROUND_UP(imw, 64) *  DIV_ROUND_UP(imh, 64) * 256) + \
+         (DIV_ROUND_UP((mbw) * (mbh), 32) * 16))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V6(w, h)     (((w) * 192) + 64)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_DEC_V6(w, h) \
+                        ((w) * ((h) * 64 + 144) + (2048/16 * (h) * 64) + \
+                         (2048/16 * 256 + 8320))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VC1_DEC_V6(w, h) \
+                                                (2096 * ((w) + (h) + 1))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H263_DEC_V6(w, h)     ((w) * 400)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V6(w, h) \
+                        ((w) * 32 + (h) * 128 + (((w) + 1) / 2) * 64 + 2112)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V6(w, h) \
+                        (((w) * 64) + (((w) + 1) * 16) + (4096 * 16))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_ENC_V6(w, h) \
+                        (((w) * 16) + (((w) + 1) * 16))
+
+/* MFC Context buffer sizes */
+#define MFC_CTX_BUF_SIZE_V6             (28 * SZ_1K)    /*  28KB */
+#define MFC_H264_DEC_CTX_BUF_SIZE_V6    (2 * SZ_1M)     /*  2MB */
+#define MFC_OTHER_DEC_CTX_BUF_SIZE_V6   (20 * SZ_1K)    /*  20KB */
+#define MFC_H264_ENC_CTX_BUF_SIZE_V6    (100 * SZ_1K)   /* 100KB */
+#define MFC_OTHER_ENC_CTX_BUF_SIZE_V6   (12 * SZ_1K)    /*  12KB */
+
+/* MFCv6 variant defines */
+#define MAX_FW_SIZE_V6                  (SZ_1M)         /* 1MB */
+#define MAX_CPB_SIZE_V6                 (3 * SZ_1M)     /* 3MB */
+#define MFC_VERSION_V6                  0x61
+#define MFC_NUM_PORTS_V6                1
+
+#endif /* _REGS_FIMV_V6_H */
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc.h b/drivers/media/platform/s5p-mfc/regs-mfc.h
index a19bece41ba9..9319e93599ae 100644
--- a/drivers/media/platform/s5p-mfc/regs-mfc.h
+++ b/drivers/media/platform/s5p-mfc/regs-mfc.h
@@ -12,6 +12,9 @@
 #ifndef _REGS_FIMV_H
 #define _REGS_FIMV_H
 
+#include <linux/kernel.h>
+#include <linux/sizes.h>
+
 #define S5P_FIMV_REG_SIZE       (S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR)
 #define S5P_FIMV_REG_COUNT      ((S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR) / 4)
 
@@ -144,6 +147,7 @@
 #define S5P_FIMV_ENC_PROFILE_H264_MAIN                  0
 #define S5P_FIMV_ENC_PROFILE_H264_HIGH                  1
 #define S5P_FIMV_ENC_PROFILE_H264_BASELINE              2
+#define S5P_FIMV_ENC_PROFILE_H264_CONSTRAINED_BASELINE  3
 #define S5P_FIMV_ENC_PROFILE_MPEG4_SIMPLE               0
 #define S5P_FIMV_ENC_PROFILE_MPEG4_ADVANCED_SIMPLE      1
 #define S5P_FIMV_ENC_PIC_STRUCT         0x083c /* picture field/frame flag */
@@ -213,6 +217,7 @@
 #define S5P_FIMV_DEC_STATUS_RESOLUTION_MASK             (3<<4)
 #define S5P_FIMV_DEC_STATUS_RESOLUTION_INC              (1<<4)
 #define S5P_FIMV_DEC_STATUS_RESOLUTION_DEC              (2<<4)
+#define S5P_FIMV_DEC_STATUS_RESOLUTION_SHIFT            4
 
 /* Decode frame address */
 #define S5P_FIMV_DECODE_Y_ADR                   0x2024
@@ -377,6 +382,16 @@
 #define S5P_FIMV_R2H_CMD_EDFU_INIT_RET          16
 #define S5P_FIMV_R2H_CMD_ERR_RET                32
 
+/* Dummy definition for MFCv6 compatibilty */
+#define S5P_FIMV_CODEC_H264_MVC_DEC             -1
+#define S5P_FIMV_R2H_CMD_FIELD_DONE_RET         -1
+#define S5P_FIMV_MFC_RESET                      -1
+#define S5P_FIMV_RISC_ON                        -1
+#define S5P_FIMV_RISC_BASE_ADDRESS              -1
+#define S5P_FIMV_CODEC_VP8_DEC                  -1
+#define S5P_FIMV_REG_CLEAR_BEGIN                0
+#define S5P_FIMV_REG_CLEAR_COUNT                0
+
 /* Error handling defines */
 #define S5P_FIMV_ERR_WARNINGS_START             145
 #define S5P_FIMV_ERR_DEC_MASK                   0xFFFF
@@ -414,5 +429,31 @@
 #define S5P_FIMV_SHARED_EXTENDED_SAR            0x0078
 #define S5P_FIMV_SHARED_H264_I_PERIOD           0x009C
 #define S5P_FIMV_SHARED_RC_CONTROL_CONFIG       0x00A0
+#define S5P_FIMV_SHARED_DISP_FRAME_TYPE_SHIFT   2
+
+/* Offset used by the hardware to store addresses */
+#define MFC_OFFSET_SHIFT        11
+
+#define FIRMWARE_ALIGN          (128 * SZ_1K)   /* 128KB */
+#define MFC_H264_CTX_BUF_SIZE   (600 * SZ_1K)   /* 600KB per H264 instance */
+#define MFC_CTX_BUF_SIZE        (10 * SZ_1K)    /* 10KB per instance */
+#define DESC_BUF_SIZE           (128 * SZ_1K)   /* 128KB for DESC buffer */
+#define SHARED_BUF_SIZE         (8 * SZ_1K)     /* 8KB for shared buffer */
+
+#define DEF_CPB_SIZE            (256 * SZ_1K)   /* 256KB */
+#define MAX_CPB_SIZE            (4 * SZ_1M)     /* 4MB */
+#define MAX_FW_SIZE             (384 * SZ_1K)
+
+#define MFC_VERSION             0x51
+#define MFC_NUM_PORTS           2
+
+#define S5P_FIMV_SHARED_FRAME_PACK_SEI_AVAIL    0x16C
+#define S5P_FIMV_SHARED_FRAME_PACK_ARRGMENT_ID  0x170
+#define S5P_FIMV_SHARED_FRAME_PACK_SEI_INFO     0x174
+#define S5P_FIMV_SHARED_FRAME_PACK_GRID_POS     0x178
+
+/* Values for resolution change in display status */
+#define S5P_FIMV_RES_INCREASE   1
+#define S5P_FIMV_RES_DECREASE   2
 
 #endif /* _REGS_FIMV_H */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index 5587ef15ca4f..130f4ac8649e 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -22,15 +22,15 @@
 #include <media/v4l2-event.h>
 #include <linux/workqueue.h>
 #include <media/videobuf2-core.h>
-#include "regs-mfc.h"
+#include "s5p_mfc_common.h"
 #include "s5p_mfc_ctrl.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_dec.h"
 #include "s5p_mfc_enc.h"
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
+#include "s5p_mfc_cmd.h"
 #include "s5p_mfc_pm.h"
-#include "s5p_mfc_shm.h"
 
 #define S5P_MFC_NAME            "s5p-mfc"
 #define S5P_MFC_DEC_NAME        "s5p-mfc-dec"
@@ -149,10 +149,12 @@ static void s5p_mfc_watchdog_worker(struct work_struct *work)
                 if (!ctx)
                         continue;
                 ctx->state = MFCINST_ERROR;
-                s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
-                s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
+                                &ctx->vq_dst);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+                                &ctx->vq_src);
                 clear_work_bit(ctx);
-                wake_up_ctx(ctx, S5P_FIMV_R2H_CMD_ERR_RET, 0);
+                wake_up_ctx(ctx, S5P_MFC_R2H_CMD_ERR_RET, 0);
         }
         clear_bit(0, &dev->hw_lock);
         spin_unlock_irqrestore(&dev->irqlock, flags);
@@ -199,6 +201,7 @@ static void s5p_mfc_clear_int_flags(struct s5p_mfc_dev *dev)
 static void s5p_mfc_handle_frame_all_extracted(struct s5p_mfc_ctx *ctx)
 {
         struct s5p_mfc_buf *dst_buf;
+        struct s5p_mfc_dev *dev = ctx->dev;
 
         ctx->state = MFCINST_FINISHED;
         ctx->sequence++;
@@ -213,8 +216,8 @@ static void s5p_mfc_handle_frame_all_extracted(struct s5p_mfc_ctx *ctx)
                 ctx->dst_queue_cnt--;
                 dst_buf->b->v4l2_buf.sequence = (ctx->sequence++);
 
-                if (s5p_mfc_read_shm(ctx, PIC_TIME_TOP) ==
-                        s5p_mfc_read_shm(ctx, PIC_TIME_BOT))
+                if (s5p_mfc_hw_call(dev->mfc_ops, get_pic_type_top, ctx) ==
+                        s5p_mfc_hw_call(dev->mfc_ops, get_pic_type_bot, ctx))
                         dst_buf->b->v4l2_buf.field = V4L2_FIELD_NONE;
                 else
                         dst_buf->b->v4l2_buf.field = V4L2_FIELD_INTERLACED;
@@ -228,8 +231,11 @@ static void s5p_mfc_handle_frame_copy_time(struct s5p_mfc_ctx *ctx)
 {
         struct s5p_mfc_dev *dev = ctx->dev;
         struct s5p_mfc_buf  *dst_buf, *src_buf;
-        size_t dec_y_addr = s5p_mfc_get_dec_y_adr();
-        unsigned int frame_type = s5p_mfc_get_frame_type();
+        size_t dec_y_addr;
+        unsigned int frame_type;
+
+        dec_y_addr = s5p_mfc_hw_call(dev->mfc_ops, get_dec_y_adr, dev);
+        frame_type = s5p_mfc_hw_call(dev->mfc_ops, get_dec_frame_type, dev);
 
         /* Copy timestamp / timecode from decoded src to dst and set
            appropraite flags */
@@ -265,10 +271,13 @@ static void s5p_mfc_handle_frame_new(struct s5p_mfc_ctx *ctx, unsigned int err)
 {
         struct s5p_mfc_dev *dev = ctx->dev;
         struct s5p_mfc_buf  *dst_buf;
-        size_t dspl_y_addr = s5p_mfc_get_dspl_y_adr();
-        unsigned int frame_type = s5p_mfc_get_frame_type();
+        size_t dspl_y_addr;
+        unsigned int frame_type;
         unsigned int index;
 
+        dspl_y_addr = s5p_mfc_hw_call(dev->mfc_ops, get_dspl_y_adr, dev);
+        frame_type = s5p_mfc_hw_call(dev->mfc_ops, get_dec_frame_type, dev);
+
         /* If frame is same as previous then skip and do not dequeue */
         if (frame_type == S5P_FIMV_DECODE_FRAME_SKIPPED) {
                 if (!ctx->after_packed_pb)
@@ -285,8 +294,10 @@ static void s5p_mfc_handle_frame_new(struct s5p_mfc_ctx *ctx, unsigned int err)
                         list_del(&dst_buf->list);
                         ctx->dst_queue_cnt--;
                         dst_buf->b->v4l2_buf.sequence = ctx->sequence;
-                        if (s5p_mfc_read_shm(ctx, PIC_TIME_TOP) ==
-                                s5p_mfc_read_shm(ctx, PIC_TIME_BOT))
+                        if (s5p_mfc_hw_call(dev->mfc_ops,
+                                        get_pic_type_top, ctx) ==
+                                s5p_mfc_hw_call(dev->mfc_ops,
+                                        get_pic_type_bot, ctx))
                                 dst_buf->b->v4l2_buf.field = V4L2_FIELD_NONE;
                         else
                                 dst_buf->b->v4l2_buf.field =
@@ -317,21 +328,23 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 
         unsigned int index;
 
-        dst_frame_status = s5p_mfc_get_dspl_status()
+        dst_frame_status = s5p_mfc_hw_call(dev->mfc_ops, get_dspl_status, dev)
                                 & S5P_FIMV_DEC_STATUS_DECODING_STATUS_MASK;
-        res_change = s5p_mfc_get_dspl_status()
-                                & S5P_FIMV_DEC_STATUS_RESOLUTION_MASK;
+        res_change = (s5p_mfc_hw_call(dev->mfc_ops, get_dspl_status, dev)
+                                & S5P_FIMV_DEC_STATUS_RESOLUTION_MASK)
+                                >> S5P_FIMV_DEC_STATUS_RESOLUTION_SHIFT;
         mfc_debug(2, "Frame Status: %x\n", dst_frame_status);
         if (ctx->state == MFCINST_RES_CHANGE_INIT)
                 ctx->state = MFCINST_RES_CHANGE_FLUSH;
-        if (res_change) {
+        if (res_change == S5P_FIMV_RES_INCREASE ||
+                res_change == S5P_FIMV_RES_DECREASE) {
                 ctx->state = MFCINST_RES_CHANGE_INIT;
-                s5p_mfc_clear_int_flags(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
                 wake_up_ctx(ctx, reason, err);
                 if (test_and_clear_bit(0, &dev->hw_lock) == 0)
                         BUG();
                 s5p_mfc_clock_off();
-                s5p_mfc_try_run(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 return;
         }
         if (ctx->dpb_flush_flag)
@@ -365,9 +378,12 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
                 && !list_empty(&ctx->src_queue)) {
                 src_buf = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
                                                                 list);
-                ctx->consumed_stream += s5p_mfc_get_consumed_stream();
-                if (ctx->codec_mode != S5P_FIMV_CODEC_H264_DEC &&
-                        s5p_mfc_get_frame_type() == S5P_FIMV_DECODE_FRAME_P_FRAME
+                ctx->consumed_stream += s5p_mfc_hw_call(dev->mfc_ops,
+                                                get_consumed_stream, dev);
+                if (ctx->codec_mode != S5P_MFC_CODEC_H264_DEC &&
+                        s5p_mfc_hw_call(dev->mfc_ops,
+                                get_dec_frame_type, dev) ==
+                                        S5P_FIMV_DECODE_FRAME_P_FRAME
                                         && ctx->consumed_stream + STUFF_BYTE <
                                         src_buf->b->v4l2_planes[0].bytesused) {
                         /* Run MFC again on the same buffer */
@@ -379,7 +395,7 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
                         ctx->consumed_stream = 0;
                         list_del(&src_buf->list);
                         ctx->src_queue_cnt--;
-                        if (s5p_mfc_err_dec(err) > 0)
+                        if (s5p_mfc_hw_call(dev->mfc_ops, err_dec, err) > 0)
                                 vb2_buffer_done(src_buf->b, VB2_BUF_STATE_ERROR);
                         else
                                 vb2_buffer_done(src_buf->b, VB2_BUF_STATE_DONE);
@@ -390,12 +406,12 @@ leave_handle_frame:
         if ((ctx->src_queue_cnt == 0 && ctx->state != MFCINST_FINISHING)
                                     || ctx->dst_queue_cnt < ctx->dpb_count)
                 clear_work_bit(ctx);
-        s5p_mfc_clear_int_flags(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
         wake_up_ctx(ctx, reason, err);
         if (test_and_clear_bit(0, &dev->hw_lock) == 0)
                 BUG();
         s5p_mfc_clock_off();
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 /* Error handling for interrupt */
@@ -412,7 +428,7 @@ static void s5p_mfc_handle_error(struct s5p_mfc_ctx *ctx,
 
         dev = ctx->dev;
         mfc_err("Interrupt Error: %08x\n", err);
-        s5p_mfc_clear_int_flags(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
         wake_up_dev(dev, reason, err);
 
         /* Error recovery is dependent on the state of context */
@@ -441,9 +457,11 @@ static void s5p_mfc_handle_error(struct s5p_mfc_ctx *ctx,
                 ctx->state = MFCINST_ERROR;
                 /* Mark all dst buffers as having an error */
                 spin_lock_irqsave(&dev->irqlock, flags);
-                s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
+                                &ctx->vq_dst);
                 /* Mark all src buffers as having an error */
-                s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+                                &ctx->vq_src);
                 spin_unlock_irqrestore(&dev->irqlock, flags);
                 if (test_and_clear_bit(0, &dev->hw_lock) == 0)
                         BUG();
@@ -461,7 +479,6 @@ static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
                                  unsigned int reason, unsigned int err)
 {
         struct s5p_mfc_dev *dev;
-        unsigned int guard_width, guard_height;
 
         if (ctx == NULL)
                 return;
@@ -470,55 +487,44 @@ static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
                 if (ctx->c_ops->post_seq_start(ctx))
                         mfc_err("post_seq_start() failed\n");
         } else {
-                ctx->img_width = s5p_mfc_get_img_width();
-                ctx->img_height = s5p_mfc_get_img_height();
-
-                ctx->buf_width = ALIGN(ctx->img_width,
-                                                S5P_FIMV_NV12MT_HALIGN);
-                ctx->buf_height = ALIGN(ctx->img_height,
-                                                S5P_FIMV_NV12MT_VALIGN);
-                mfc_debug(2, "SEQ Done: Movie dimensions %dx%d, "
-                        "buffer dimensions: %dx%d\n", ctx->img_width,
-                                ctx->img_height, ctx->buf_width,
-                                                ctx->buf_height);
-                if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC) {
-                        ctx->luma_size = ALIGN(ctx->buf_width *
-                                ctx->buf_height, S5P_FIMV_DEC_BUF_ALIGN);
-                        ctx->chroma_size = ALIGN(ctx->buf_width *
-                                         ALIGN((ctx->img_height >> 1),
-                                               S5P_FIMV_NV12MT_VALIGN),
-                                               S5P_FIMV_DEC_BUF_ALIGN);
-                        ctx->mv_size = ALIGN(ctx->buf_width *
-                                        ALIGN((ctx->buf_height >> 2),
-                                        S5P_FIMV_NV12MT_VALIGN),
-                                        S5P_FIMV_DEC_BUF_ALIGN);
-                } else {
-                        guard_width = ALIGN(ctx->img_width + 24,
-                                        S5P_FIMV_NV12MT_HALIGN);
-                        guard_height = ALIGN(ctx->img_height + 16,
-                                                S5P_FIMV_NV12MT_VALIGN);
-                        ctx->luma_size = ALIGN(guard_width *
-                                guard_height, S5P_FIMV_DEC_BUF_ALIGN);
-                        guard_width = ALIGN(ctx->img_width + 16,
-                                                S5P_FIMV_NV12MT_HALIGN);
-                        guard_height = ALIGN((ctx->img_height >> 1) + 4,
-                                                S5P_FIMV_NV12MT_VALIGN);
-                        ctx->chroma_size = ALIGN(guard_width *
-                                guard_height, S5P_FIMV_DEC_BUF_ALIGN);
-                        ctx->mv_size = 0;
-                }
-                ctx->dpb_count = s5p_mfc_get_dpb_count();
+                ctx->img_width = s5p_mfc_hw_call(dev->mfc_ops, get_img_width,
+                                dev);
+                ctx->img_height = s5p_mfc_hw_call(dev->mfc_ops, get_img_height,
+                                dev);
+
+                s5p_mfc_hw_call(dev->mfc_ops, dec_calc_dpb_size, ctx);
+
+                ctx->dpb_count = s5p_mfc_hw_call(dev->mfc_ops, get_dpb_count,
+                                dev);
+                ctx->mv_count = s5p_mfc_hw_call(dev->mfc_ops, get_mv_count,
+                                dev);
                 if (ctx->img_width == 0 || ctx->img_height == 0)
                         ctx->state = MFCINST_ERROR;
                 else
                         ctx->state = MFCINST_HEAD_PARSED;
+
+                if ((ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+                        ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) &&
+                                !list_empty(&ctx->src_queue)) {
+                        struct s5p_mfc_buf *src_buf;
+                        src_buf = list_entry(ctx->src_queue.next,
+                                        struct s5p_mfc_buf, list);
+                        if (s5p_mfc_hw_call(dev->mfc_ops, get_consumed_stream,
+                                                dev) <
+                                        src_buf->b->v4l2_planes[0].bytesused)
+                                ctx->head_processed = 0;
+                        else
+                                ctx->head_processed = 1;
+                } else {
+                        ctx->head_processed = 1;
+                }
         }
-        s5p_mfc_clear_int_flags(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
         clear_work_bit(ctx);
         if (test_and_clear_bit(0, &dev->hw_lock) == 0)
                 BUG();
         s5p_mfc_clock_off();
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
         wake_up_ctx(ctx, reason, err);
 }
 
@@ -533,14 +539,14 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
         if (ctx == NULL)
                 return;
         dev = ctx->dev;
-        s5p_mfc_clear_int_flags(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
         ctx->int_type = reason;
         ctx->int_err = err;
         ctx->int_cond = 1;
         clear_work_bit(ctx);
         if (err == 0) {
                 ctx->state = MFCINST_RUNNING;
-                if (!ctx->dpb_flush_flag) {
+                if (!ctx->dpb_flush_flag && ctx->head_processed) {
                         spin_lock_irqsave(&dev->irqlock, flags);
                         if (!list_empty(&ctx->src_queue)) {
                                 src_buf = list_entry(ctx->src_queue.next,
@@ -560,7 +566,7 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
                 s5p_mfc_clock_off();
 
                 wake_up(&ctx->queue);
-                s5p_mfc_try_run(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
         } else {
                 if (test_and_clear_bit(0, &dev->hw_lock) == 0)
                         BUG();
@@ -602,7 +608,7 @@ static void s5p_mfc_handle_stream_complete(struct s5p_mfc_ctx *ctx,
 
         s5p_mfc_clock_off();
         wake_up(&ctx->queue);
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 /* Interrupt processing */
@@ -618,81 +624,83 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
         atomic_set(&dev->watchdog_cnt, 0);
         ctx = dev->ctx[dev->curr_ctx];
         /* Get the reason of interrupt and the error code */
-        reason = s5p_mfc_get_int_reason();
-        err = s5p_mfc_get_int_err();
+        reason = s5p_mfc_hw_call(dev->mfc_ops, get_int_reason, dev);
+        err = s5p_mfc_hw_call(dev->mfc_ops, get_int_err, dev);
         mfc_debug(1, "Int reason: %d (err: %08x)\n", reason, err);
         switch (reason) {
-        case S5P_FIMV_R2H_CMD_ERR_RET:
+        case S5P_MFC_R2H_CMD_ERR_RET:
                 /* An error has occured */
                 if (ctx->state == MFCINST_RUNNING &&
-                        s5p_mfc_err_dec(err) >= S5P_FIMV_ERR_WARNINGS_START)
+                        s5p_mfc_hw_call(dev->mfc_ops, err_dec, err) >=
+                                dev->warn_start)
                         s5p_mfc_handle_frame(ctx, reason, err);
                 else
                         s5p_mfc_handle_error(ctx, reason, err);
                 clear_bit(0, &dev->enter_suspend);
                 break;
 
-        case S5P_FIMV_R2H_CMD_SLICE_DONE_RET:
-        case S5P_FIMV_R2H_CMD_FRAME_DONE_RET:
+        case S5P_MFC_R2H_CMD_SLICE_DONE_RET:
+        case S5P_MFC_R2H_CMD_FIELD_DONE_RET:
+        case S5P_MFC_R2H_CMD_FRAME_DONE_RET:
                 if (ctx->c_ops->post_frame_start) {
                         if (ctx->c_ops->post_frame_start(ctx))
                                 mfc_err("post_frame_start() failed\n");
-                        s5p_mfc_clear_int_flags(dev);
+                        s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
                         wake_up_ctx(ctx, reason, err);
                         if (test_and_clear_bit(0, &dev->hw_lock) == 0)
                                 BUG();
                         s5p_mfc_clock_off();
-                        s5p_mfc_try_run(dev);
+                        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 } else {
                         s5p_mfc_handle_frame(ctx, reason, err);
                 }
                 break;
 
-        case S5P_FIMV_R2H_CMD_SEQ_DONE_RET:
+        case S5P_MFC_R2H_CMD_SEQ_DONE_RET:
                 s5p_mfc_handle_seq_done(ctx, reason, err);
                 break;
 
-        case S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET:
-                ctx->inst_no = s5p_mfc_get_inst_no();
+        case S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET:
+                ctx->inst_no = s5p_mfc_hw_call(dev->mfc_ops, get_inst_no, dev);
                 ctx->state = MFCINST_GOT_INST;
                 clear_work_bit(ctx);
                 wake_up(&ctx->queue);
                 goto irq_cleanup_hw;
 
-        case S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET:
+        case S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET:
                 clear_work_bit(ctx);
                 ctx->state = MFCINST_FREE;
                 wake_up(&ctx->queue);
                 goto irq_cleanup_hw;
 
-        case S5P_FIMV_R2H_CMD_SYS_INIT_RET:
-        case S5P_FIMV_R2H_CMD_FW_STATUS_RET:
-        case S5P_FIMV_R2H_CMD_SLEEP_RET:
-        case S5P_FIMV_R2H_CMD_WAKEUP_RET:
+        case S5P_MFC_R2H_CMD_SYS_INIT_RET:
+        case S5P_MFC_R2H_CMD_FW_STATUS_RET:
+        case S5P_MFC_R2H_CMD_SLEEP_RET:
+        case S5P_MFC_R2H_CMD_WAKEUP_RET:
                 if (ctx)
                         clear_work_bit(ctx);
-                s5p_mfc_clear_int_flags(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
                 wake_up_dev(dev, reason, err);
                 clear_bit(0, &dev->hw_lock);
                 clear_bit(0, &dev->enter_suspend);
                 break;
 
-        case S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET:
+        case S5P_MFC_R2H_CMD_INIT_BUFFERS_RET:
                 s5p_mfc_handle_init_buffers(ctx, reason, err);
                 break;
 
-        case S5P_FIMV_R2H_CMD_ENC_COMPLETE_RET:
+        case S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET:
                 s5p_mfc_handle_stream_complete(ctx, reason, err);
                 break;
 
         default:
                 mfc_debug(2, "Unknown int reason\n");
-                s5p_mfc_clear_int_flags(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
         }
         mfc_debug_leave();
         return IRQ_HANDLED;
 irq_cleanup_hw:
-        s5p_mfc_clear_int_flags(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
         ctx->int_type = reason;
         ctx->int_err = err;
         ctx->int_cond = 1;
@@ -701,7 +709,7 @@ irq_cleanup_hw:
 
         s5p_mfc_clock_off();
 
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
         mfc_debug(2, "Exit via irq_cleanup_hw\n");
         return IRQ_HANDLED;
 }
@@ -749,6 +757,7 @@ static int s5p_mfc_open(struct file *file)
         if (s5p_mfc_get_node_type(file) == MFCNODE_DECODER) {
                 ctx->type = MFCINST_DECODER;
                 ctx->c_ops = get_dec_codec_ops();
+                s5p_mfc_dec_init(ctx);
                 /* Setup ctrl handler */
                 ret = s5p_mfc_dec_ctrls_setup(ctx);
                 if (ret) {
@@ -761,6 +770,7 @@ static int s5p_mfc_open(struct file *file)
                 /* only for encoder */
                 INIT_LIST_HEAD(&ctx->ref_queue);
                 ctx->ref_queue_cnt = 0;
+                s5p_mfc_enc_init(ctx);
                 /* Setup ctrl handler */
                 ret = s5p_mfc_enc_ctrls_setup(ctx);
                 if (ret) {
@@ -886,19 +896,20 @@ static int s5p_mfc_release(struct file *file)
                 ctx->state = MFCINST_RETURN_INST;
                 set_work_bit_irqsave(ctx);
                 s5p_mfc_clean_ctx_int_flags(ctx);
-                s5p_mfc_try_run(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 /* Wait until instance is returned or timeout occured */
                 if (s5p_mfc_wait_for_done_ctx
-                    (ctx, S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET, 0)) {
+                    (ctx, S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET, 0)) {
                         s5p_mfc_clock_off();
                         mfc_err("Err returning instance\n");
                 }
                 mfc_debug(2, "After free instance\n");
                 /* Free resources */
-                s5p_mfc_release_codec_buffers(ctx);
-                s5p_mfc_release_instance_buffer(ctx);
+                s5p_mfc_hw_call(dev->mfc_ops, release_codec_buffers, ctx);
+                s5p_mfc_hw_call(dev->mfc_ops, release_instance_buffer, ctx);
                 if (ctx->type == MFCINST_DECODER)
-                        s5p_mfc_release_dec_desc_buffer(ctx);
+                        s5p_mfc_hw_call(dev->mfc_ops, release_dec_desc_buffer,
+                                        ctx);
 
                 ctx->inst_no = MFC_NO_INSTANCE_SET;
         }
@@ -910,6 +921,7 @@ static int s5p_mfc_release(struct file *file)
                 mfc_debug(2, "Last instance - release firmware\n");
                 /* reset <-> F/W release */
                 s5p_mfc_reset(dev);
+                s5p_mfc_deinit_hw(dev);
                 s5p_mfc_release_firmware(dev);
                 del_timer_sync(&dev->watchdog_timer);
                 if (s5p_mfc_power_off() < 0)
@@ -1041,6 +1053,9 @@ static int s5p_mfc_probe(struct platform_device *pdev)
                 return -ENODEV;
         }
 
+        dev->variant = (struct s5p_mfc_variant *)
+                platform_get_device_id(pdev)->driver_data;
+
         ret = s5p_mfc_init_pm(dev);
         if (ret < 0) {
                 dev_err(&pdev->dev, "failed to get mfc clock source\n");
@@ -1076,6 +1091,7 @@ static int s5p_mfc_probe(struct platform_device *pdev)
                 ret = -ENODEV;
                 goto err_res;
         }
+
         dev->mem_dev_r = device_find_child(&dev->plat_dev->dev, "s5p-mfc-r",
                                            match_child);
         if (!dev->mem_dev_r) {
@@ -1139,6 +1155,7 @@ static int s5p_mfc_probe(struct platform_device *pdev)
         vfd->release    = video_device_release,
         vfd->lock       = &dev->mfc_mutex;
         vfd->v4l2_dev   = &dev->v4l2_dev;
+        vfd->vfl_dir    = VFL_DIR_M2M;
         snprintf(vfd->name, sizeof(vfd->name), "%s", S5P_MFC_ENC_NAME);
         dev->vfd_enc    = vfd;
         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
@@ -1160,6 +1177,10 @@ static int s5p_mfc_probe(struct platform_device *pdev)
         dev->watchdog_timer.data = (unsigned long)dev;
         dev->watchdog_timer.function = s5p_mfc_watchdog;
 
+        /* Initialize HW ops and commands based on MFC version */
+        s5p_mfc_init_hw_ops(dev);
+        s5p_mfc_init_hw_cmds(dev);
+
         pr_debug("%s--\n", __func__);
         return 0;
 
@@ -1280,9 +1301,78 @@ static const struct dev_pm_ops s5p_mfc_pm_ops = {
                            NULL)
 };
 
+struct s5p_mfc_buf_size_v5 mfc_buf_size_v5 = {
+        .h264_ctx       = MFC_H264_CTX_BUF_SIZE,
+        .non_h264_ctx   = MFC_CTX_BUF_SIZE,
+        .dsc            = DESC_BUF_SIZE,
+        .shm            = SHARED_BUF_SIZE,
+};
+
+struct s5p_mfc_buf_size buf_size_v5 = {
+        .fw     = MAX_FW_SIZE,
+        .cpb    = MAX_CPB_SIZE,
+        .priv   = &mfc_buf_size_v5,
+};
+
+struct s5p_mfc_buf_align mfc_buf_align_v5 = {
+        .base = MFC_BASE_ALIGN_ORDER,
+};
+
+static struct s5p_mfc_variant mfc_drvdata_v5 = {
+        .version        = MFC_VERSION,
+        .port_num       = MFC_NUM_PORTS,
+        .buf_size       = &buf_size_v5,
+        .buf_align      = &mfc_buf_align_v5,
+        .mclk_name      = "sclk_mfc",
+        .fw_name        = "s5p-mfc.fw",
+};
+
+struct s5p_mfc_buf_size_v6 mfc_buf_size_v6 = {
+        .dev_ctx        = MFC_CTX_BUF_SIZE_V6,
+        .h264_dec_ctx   = MFC_H264_DEC_CTX_BUF_SIZE_V6,
+        .other_dec_ctx  = MFC_OTHER_DEC_CTX_BUF_SIZE_V6,
+        .h264_enc_ctx   = MFC_H264_ENC_CTX_BUF_SIZE_V6,
+        .other_enc_ctx  = MFC_OTHER_ENC_CTX_BUF_SIZE_V6,
+};
+
+struct s5p_mfc_buf_size buf_size_v6 = {
+        .fw     = MAX_FW_SIZE_V6,
+        .cpb    = MAX_CPB_SIZE_V6,
+        .priv   = &mfc_buf_size_v6,
+};
+
+struct s5p_mfc_buf_align mfc_buf_align_v6 = {
+        .base = 0,
+};
+
+static struct s5p_mfc_variant mfc_drvdata_v6 = {
+        .version        = MFC_VERSION_V6,
+        .port_num       = MFC_NUM_PORTS_V6,
+        .buf_size       = &buf_size_v6,
+        .buf_align      = &mfc_buf_align_v6,
+        .mclk_name      = "aclk_333",
+        .fw_name        = "s5p-mfc-v6.fw",
+};
+
+static struct platform_device_id mfc_driver_ids[] = {
+        {
+                .name = "s5p-mfc",
+                .driver_data = (unsigned long)&mfc_drvdata_v5,
+        }, {
+                .name = "s5p-mfc-v5",
+                .driver_data = (unsigned long)&mfc_drvdata_v5,
+        }, {
+                .name = "s5p-mfc-v6",
+                .driver_data = (unsigned long)&mfc_drvdata_v6,
+        },
+        {},
+};
+MODULE_DEVICE_TABLE(platform, mfc_driver_ids);
+
 static struct platform_driver s5p_mfc_driver = {
-        .probe  = s5p_mfc_probe,
-        .remove = __devexit_p(s5p_mfc_remove),
+        .probe          = s5p_mfc_probe,
+        .remove         = __devexit_p(s5p_mfc_remove),
+        .id_table       = mfc_driver_ids,
         .driver = {
                 .name   = S5P_MFC_NAME,
                 .owner  = THIS_MODULE,
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
index 91a415573bd2..f0a41c95df84 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
@@ -1,7 +1,7 @@
 /*
  * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
  *
- * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
  *              http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
@@ -10,111 +10,20 @@
  * (at your option) any later version.
  */
 
-#include "regs-mfc.h"
 #include "s5p_mfc_cmd.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
+#include "s5p_mfc_cmd_v5.h"
+#include "s5p_mfc_cmd_v6.h"
 
-/* This function is used to send a command to the MFC */
-static int s5p_mfc_cmd_host2risc(struct s5p_mfc_dev *dev, int cmd,
-                                                struct s5p_mfc_cmd_args *args)
-{
-        int cur_cmd;
-        unsigned long timeout;
-
-        timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
-        /* wait until host to risc command register becomes 'H2R_CMD_EMPTY' */
-        do {
-                if (time_after(jiffies, timeout)) {
-                        mfc_err("Timeout while waiting for hardware\n");
-                        return -EIO;
-                }
-                cur_cmd = mfc_read(dev, S5P_FIMV_HOST2RISC_CMD);
-        } while (cur_cmd != S5P_FIMV_H2R_CMD_EMPTY);
-        mfc_write(dev, args->arg[0], S5P_FIMV_HOST2RISC_ARG1);
-        mfc_write(dev, args->arg[1], S5P_FIMV_HOST2RISC_ARG2);
-        mfc_write(dev, args->arg[2], S5P_FIMV_HOST2RISC_ARG3);
-        mfc_write(dev, args->arg[3], S5P_FIMV_HOST2RISC_ARG4);
-        /* Issue the command */
-        mfc_write(dev, cmd, S5P_FIMV_HOST2RISC_CMD);
-        return 0;
-}
-
-/* Initialize the MFC */
-int s5p_mfc_sys_init_cmd(struct s5p_mfc_dev *dev)
-{
-        struct s5p_mfc_cmd_args h2r_args;
-
-        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-        h2r_args.arg[0] = dev->fw_size;
-        return s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_SYS_INIT, &h2r_args);
-}
-
-/* Suspend the MFC hardware */
-int s5p_mfc_sleep_cmd(struct s5p_mfc_dev *dev)
-{
-        struct s5p_mfc_cmd_args h2r_args;
-
-        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-        return s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_SLEEP, &h2r_args);
-}
+static struct s5p_mfc_hw_cmds *s5p_mfc_cmds;
 
-/* Wake up the MFC hardware */
-int s5p_mfc_wakeup_cmd(struct s5p_mfc_dev *dev)
+void s5p_mfc_init_hw_cmds(struct s5p_mfc_dev *dev)
 {
-        struct s5p_mfc_cmd_args h2r_args;
+        if (IS_MFCV6(dev))
+                s5p_mfc_cmds = s5p_mfc_init_hw_cmds_v6();
+        else
+                s5p_mfc_cmds = s5p_mfc_init_hw_cmds_v5();
 
-        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-        return s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_WAKEUP, &h2r_args);
+        dev->mfc_cmds = s5p_mfc_cmds;
 }
-
-
-int s5p_mfc_open_inst_cmd(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_cmd_args h2r_args;
-        int ret;
-
-        /* Preparing decoding - getting instance number */
-        mfc_debug(2, "Getting instance number (codec: %d)\n", ctx->codec_mode);
-        dev->curr_ctx = ctx->num;
-        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-        h2r_args.arg[0] = ctx->codec_mode;
-        h2r_args.arg[1] = 0; /* no crc & no pixelcache */
-        h2r_args.arg[2] = ctx->ctx_ofs;
-        h2r_args.arg[3] = ctx->ctx_size;
-        ret = s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_OPEN_INSTANCE,
-                                                                &h2r_args);
-        if (ret) {
-                mfc_err("Failed to create a new instance\n");
-                ctx->state = MFCINST_ERROR;
-        }
-        return ret;
-}
-
-int s5p_mfc_close_inst_cmd(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_cmd_args h2r_args;
-        int ret;
-
-        if (ctx->state == MFCINST_FREE) {
-                mfc_err("Instance already returned\n");
-                ctx->state = MFCINST_ERROR;
-                return -EINVAL;
-        }
-        /* Closing decoding instance  */
-        mfc_debug(2, "Returning instance number %d\n", ctx->inst_no);
-        dev->curr_ctx = ctx->num;
-        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-        h2r_args.arg[0] = ctx->inst_no;
-        ret = s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_CLOSE_INSTANCE,
-                                                                &h2r_args);
-        if (ret) {
-                mfc_err("Failed to return an instance\n");
-                ctx->state = MFCINST_ERROR;
-                return -EINVAL;
-        }
-        return 0;
-}
-
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
index 8b090d3723e7..282e6c780702 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
@@ -1,7 +1,7 @@
 /*
  * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
  *
- * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
  *              http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
@@ -21,10 +21,15 @@ struct s5p_mfc_cmd_args {
         unsigned int    arg[MAX_H2R_ARG];
 };
 
-int s5p_mfc_sys_init_cmd(struct s5p_mfc_dev *dev);
-int s5p_mfc_sleep_cmd(struct s5p_mfc_dev *dev);
-int s5p_mfc_wakeup_cmd(struct s5p_mfc_dev *dev);
-int s5p_mfc_open_inst_cmd(struct s5p_mfc_ctx *ctx);
-int s5p_mfc_close_inst_cmd(struct s5p_mfc_ctx *ctx);
+struct s5p_mfc_hw_cmds {
+        int (*cmd_host2risc)(struct s5p_mfc_dev *dev, int cmd,
+                                struct s5p_mfc_cmd_args *args);
+        int (*sys_init_cmd)(struct s5p_mfc_dev *dev);
+        int (*sleep_cmd)(struct s5p_mfc_dev *dev);
+        int (*wakeup_cmd)(struct s5p_mfc_dev *dev);
+        int (*open_inst_cmd)(struct s5p_mfc_ctx *ctx);
+        int (*close_inst_cmd)(struct s5p_mfc_ctx *ctx);
+};
 
+void s5p_mfc_init_hw_cmds(struct s5p_mfc_dev *dev);
 #endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
new file mode 100644
index 000000000000..138778083c63
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
@@ -0,0 +1,166 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "regs-mfc.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+
+/* This function is used to send a command to the MFC */
+int s5p_mfc_cmd_host2risc_v5(struct s5p_mfc_dev *dev, int cmd,
+                                struct s5p_mfc_cmd_args *args)
+{
+        int cur_cmd;
+        unsigned long timeout;
+
+        timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
+        /* wait until host to risc command register becomes 'H2R_CMD_EMPTY' */
+        do {
+                if (time_after(jiffies, timeout)) {
+                        mfc_err("Timeout while waiting for hardware\n");
+                        return -EIO;
+                }
+                cur_cmd = mfc_read(dev, S5P_FIMV_HOST2RISC_CMD);
+        } while (cur_cmd != S5P_FIMV_H2R_CMD_EMPTY);
+        mfc_write(dev, args->arg[0], S5P_FIMV_HOST2RISC_ARG1);
+        mfc_write(dev, args->arg[1], S5P_FIMV_HOST2RISC_ARG2);
+        mfc_write(dev, args->arg[2], S5P_FIMV_HOST2RISC_ARG3);
+        mfc_write(dev, args->arg[3], S5P_FIMV_HOST2RISC_ARG4);
+        /* Issue the command */
+        mfc_write(dev, cmd, S5P_FIMV_HOST2RISC_CMD);
+        return 0;
+}
+
+/* Initialize the MFC */
+int s5p_mfc_sys_init_cmd_v5(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        h2r_args.arg[0] = dev->fw_size;
+        return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_SYS_INIT,
+                        &h2r_args);
+}
+
+/* Suspend the MFC hardware */
+int s5p_mfc_sleep_cmd_v5(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_SLEEP, &h2r_args);
+}
+
+/* Wake up the MFC hardware */
+int s5p_mfc_wakeup_cmd_v5(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_WAKEUP,
+                        &h2r_args);
+}
+
+
+int s5p_mfc_open_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_cmd_args h2r_args;
+        int ret;
+
+        /* Preparing decoding - getting instance number */
+        mfc_debug(2, "Getting instance number (codec: %d)\n", ctx->codec_mode);
+        dev->curr_ctx = ctx->num;
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_DEC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_H264_DEC;
+                break;
+        case S5P_MFC_CODEC_VC1_DEC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_VC1_DEC;
+                break;
+        case S5P_MFC_CODEC_MPEG4_DEC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_MPEG4_DEC;
+                break;
+        case S5P_MFC_CODEC_MPEG2_DEC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_MPEG2_DEC;
+                break;
+        case S5P_MFC_CODEC_H263_DEC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_H263_DEC;
+                break;
+        case S5P_MFC_CODEC_VC1RCV_DEC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_VC1RCV_DEC;
+                break;
+        case S5P_MFC_CODEC_H264_ENC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_H264_ENC;
+                break;
+        case S5P_MFC_CODEC_MPEG4_ENC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_MPEG4_ENC;
+                break;
+        case S5P_MFC_CODEC_H263_ENC:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_H263_ENC;
+                break;
+        default:
+                h2r_args.arg[0] = S5P_FIMV_CODEC_NONE;
+        };
+        h2r_args.arg[1] = 0; /* no crc & no pixelcache */
+        h2r_args.arg[2] = ctx->ctx.ofs;
+        h2r_args.arg[3] = ctx->ctx.size;
+        ret = s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_OPEN_INSTANCE,
+                                                                &h2r_args);
+        if (ret) {
+                mfc_err("Failed to create a new instance\n");
+                ctx->state = MFCINST_ERROR;
+        }
+        return ret;
+}
+
+int s5p_mfc_close_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_cmd_args h2r_args;
+        int ret;
+
+        if (ctx->state == MFCINST_FREE) {
+                mfc_err("Instance already returned\n");
+                ctx->state = MFCINST_ERROR;
+                return -EINVAL;
+        }
+        /* Closing decoding instance  */
+        mfc_debug(2, "Returning instance number %d\n", ctx->inst_no);
+        dev->curr_ctx = ctx->num;
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        h2r_args.arg[0] = ctx->inst_no;
+        ret = s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_CLOSE_INSTANCE,
+                                                                &h2r_args);
+        if (ret) {
+                mfc_err("Failed to return an instance\n");
+                ctx->state = MFCINST_ERROR;
+                return -EINVAL;
+        }
+        return 0;
+}
+
+/* Initialize cmd function pointers for MFC v5 */
+static struct s5p_mfc_hw_cmds s5p_mfc_cmds_v5 = {
+        .cmd_host2risc = s5p_mfc_cmd_host2risc_v5,
+        .sys_init_cmd = s5p_mfc_sys_init_cmd_v5,
+        .sleep_cmd = s5p_mfc_sleep_cmd_v5,
+        .wakeup_cmd = s5p_mfc_wakeup_cmd_v5,
+        .open_inst_cmd = s5p_mfc_open_inst_cmd_v5,
+        .close_inst_cmd = s5p_mfc_close_inst_cmd_v5,
+};
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v5(void)
+{
+        return &s5p_mfc_cmds_v5;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h
new file mode 100644
index 000000000000..6928a5514c1b
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h
@@ -0,0 +1,20 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef S5P_MFC_CMD_V5_H_
+#define S5P_MFC_CMD_V5_H_
+
+#include "s5p_mfc_common.h"
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v5(void);
+
+#endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
new file mode 100644
index 000000000000..754bfbcb1c43
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
@@ -0,0 +1,156 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "s5p_mfc_common.h"
+
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
+
+int s5p_mfc_cmd_host2risc_v6(struct s5p_mfc_dev *dev, int cmd,
+                                struct s5p_mfc_cmd_args *args)
+{
+        mfc_debug(2, "Issue the command: %d\n", cmd);
+
+        /* Reset RISC2HOST command */
+        mfc_write(dev, 0x0, S5P_FIMV_RISC2HOST_CMD_V6);
+
+        /* Issue the command */
+        mfc_write(dev, cmd, S5P_FIMV_HOST2RISC_CMD_V6);
+        mfc_write(dev, 0x1, S5P_FIMV_HOST2RISC_INT_V6);
+
+        return 0;
+}
+
+int s5p_mfc_sys_init_cmd_v6(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+        struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
+
+        s5p_mfc_hw_call(dev->mfc_ops, alloc_dev_context_buffer, dev);
+        mfc_write(dev, dev->ctx_buf.dma, S5P_FIMV_CONTEXT_MEM_ADDR_V6);
+        mfc_write(dev, buf_size->dev_ctx, S5P_FIMV_CONTEXT_MEM_SIZE_V6);
+        return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_SYS_INIT_V6,
+                                        &h2r_args);
+}
+
+int s5p_mfc_sleep_cmd_v6(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_SLEEP_V6,
+                        &h2r_args);
+}
+
+int s5p_mfc_wakeup_cmd_v6(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_cmd_args h2r_args;
+
+        memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+        return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_WAKEUP_V6,
+                                        &h2r_args);
+}
+
+/* Open a new instance and get its number */
+int s5p_mfc_open_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_cmd_args h2r_args;
+        int codec_type;
+
+        mfc_debug(2, "Requested codec mode: %d\n", ctx->codec_mode);
+        dev->curr_ctx = ctx->num;
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_DEC:
+                codec_type = S5P_FIMV_CODEC_H264_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_H264_MVC_DEC:
+                codec_type = S5P_FIMV_CODEC_H264_MVC_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_VC1_DEC:
+                codec_type = S5P_FIMV_CODEC_VC1_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_MPEG4_DEC:
+                codec_type = S5P_FIMV_CODEC_MPEG4_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_MPEG2_DEC:
+                codec_type = S5P_FIMV_CODEC_MPEG2_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_H263_DEC:
+                codec_type = S5P_FIMV_CODEC_H263_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_VC1RCV_DEC:
+                codec_type = S5P_FIMV_CODEC_VC1RCV_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_VP8_DEC:
+                codec_type = S5P_FIMV_CODEC_VP8_DEC_V6;
+                break;
+        case S5P_MFC_CODEC_H264_ENC:
+                codec_type = S5P_FIMV_CODEC_H264_ENC_V6;
+                break;
+        case S5P_MFC_CODEC_H264_MVC_ENC:
+                codec_type = S5P_FIMV_CODEC_H264_MVC_ENC_V6;
+                break;
+        case S5P_MFC_CODEC_MPEG4_ENC:
+                codec_type = S5P_FIMV_CODEC_MPEG4_ENC_V6;
+                break;
+        case S5P_MFC_CODEC_H263_ENC:
+                codec_type = S5P_FIMV_CODEC_H263_ENC_V6;
+                break;
+        default:
+                codec_type = S5P_FIMV_CODEC_NONE_V6;
+        };
+        mfc_write(dev, codec_type, S5P_FIMV_CODEC_TYPE_V6);
+        mfc_write(dev, ctx->ctx.dma, S5P_FIMV_CONTEXT_MEM_ADDR_V6);
+        mfc_write(dev, ctx->ctx.size, S5P_FIMV_CONTEXT_MEM_SIZE_V6);
+        mfc_write(dev, 0, S5P_FIMV_D_CRC_CTRL_V6); /* no crc */
+
+        return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_OPEN_INSTANCE_V6,
+                                        &h2r_args);
+}
+
+/* Close instance */
+int s5p_mfc_close_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_cmd_args h2r_args;
+        int ret = 0;
+
+        dev->curr_ctx = ctx->num;
+        if (ctx->state != MFCINST_FREE) {
+                mfc_write(dev, ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+                ret = s5p_mfc_cmd_host2risc_v6(dev,
+                                        S5P_FIMV_H2R_CMD_CLOSE_INSTANCE_V6,
+                                        &h2r_args);
+        } else {
+                ret = -EINVAL;
+        }
+
+        return ret;
+}
+
+/* Initialize cmd function pointers for MFC v6 */
+static struct s5p_mfc_hw_cmds s5p_mfc_cmds_v6 = {
+        .cmd_host2risc = s5p_mfc_cmd_host2risc_v6,
+        .sys_init_cmd = s5p_mfc_sys_init_cmd_v6,
+        .sleep_cmd = s5p_mfc_sleep_cmd_v6,
+        .wakeup_cmd = s5p_mfc_wakeup_cmd_v6,
+        .open_inst_cmd = s5p_mfc_open_inst_cmd_v6,
+        .close_inst_cmd = s5p_mfc_close_inst_cmd_v6,
+};
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v6(void)
+{
+        return &s5p_mfc_cmds_v6;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h
new file mode 100644
index 000000000000..b7a8e57837b5
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h
@@ -0,0 +1,20 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef S5P_MFC_CMD_V6_H_
+#define S5P_MFC_CMD_V6_H_
+
+#include "s5p_mfc_common.h"
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v6(void);
+
+#endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
index 519b0d66d8d1..f02e0497ca98 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
@@ -16,13 +16,14 @@
 #ifndef S5P_MFC_COMMON_H_
 #define S5P_MFC_COMMON_H_
 
-#include "regs-mfc.h"
 #include <linux/platform_device.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/videobuf2-core.h>
+#include "regs-mfc.h"
+#include "regs-mfc-v6.h"
 
 /* Definitions related to MFC memory */
 
@@ -30,17 +31,6 @@
 *  while mmaping */
 #define DST_QUEUE_OFF_BASE      (TASK_SIZE / 2)
 
-/* Offset used by the hardware to store addresses */
-#define MFC_OFFSET_SHIFT        11
-
-#define FIRMWARE_ALIGN          0x20000         /* 128KB */
-#define MFC_H264_CTX_BUF_SIZE   0x96000         /* 600KB per H264 instance */
-#define MFC_CTX_BUF_SIZE        0x2800          /* 10KB per instance */
-#define DESC_BUF_SIZE           0x20000         /* 128KB for DESC buffer */
-#define SHARED_BUF_SIZE         0x2000          /* 8KB for shared buffer */
-
-#define DEF_CPB_SIZE            0x40000         /* 512KB */
-
 #define MFC_BANK1_ALLOC_CTX     0
 #define MFC_BANK2_ALLOC_CTX     1
 
@@ -74,7 +64,40 @@ static inline dma_addr_t s5p_mfc_mem_cookie(void *a, void *b)
 #define MFC_ENC_CAP_PLANE_COUNT 1
 #define MFC_ENC_OUT_PLANE_COUNT 2
 #define STUFF_BYTE              4
-#define MFC_MAX_CTRLS           64
+#define MFC_MAX_CTRLS           70
+
+#define S5P_MFC_CODEC_NONE              -1
+#define S5P_MFC_CODEC_H264_DEC          0
+#define S5P_MFC_CODEC_H264_MVC_DEC      1
+#define S5P_MFC_CODEC_VC1_DEC           2
+#define S5P_MFC_CODEC_MPEG4_DEC         3
+#define S5P_MFC_CODEC_MPEG2_DEC         4
+#define S5P_MFC_CODEC_H263_DEC          5
+#define S5P_MFC_CODEC_VC1RCV_DEC        6
+#define S5P_MFC_CODEC_VP8_DEC           7
+
+#define S5P_MFC_CODEC_H264_ENC          20
+#define S5P_MFC_CODEC_H264_MVC_ENC      21
+#define S5P_MFC_CODEC_MPEG4_ENC         22
+#define S5P_MFC_CODEC_H263_ENC          23
+
+#define S5P_MFC_R2H_CMD_EMPTY                   0
+#define S5P_MFC_R2H_CMD_SYS_INIT_RET            1
+#define S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET       2
+#define S5P_MFC_R2H_CMD_SEQ_DONE_RET            3
+#define S5P_MFC_R2H_CMD_INIT_BUFFERS_RET        4
+#define S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET      6
+#define S5P_MFC_R2H_CMD_SLEEP_RET               7
+#define S5P_MFC_R2H_CMD_WAKEUP_RET              8
+#define S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET        9
+#define S5P_MFC_R2H_CMD_DPB_FLUSH_RET           10
+#define S5P_MFC_R2H_CMD_NAL_ABORT_RET           11
+#define S5P_MFC_R2H_CMD_FW_STATUS_RET           12
+#define S5P_MFC_R2H_CMD_FRAME_DONE_RET          13
+#define S5P_MFC_R2H_CMD_FIELD_DONE_RET          14
+#define S5P_MFC_R2H_CMD_SLICE_DONE_RET          15
+#define S5P_MFC_R2H_CMD_ENC_BUFFER_FUL_RET      16
+#define S5P_MFC_R2H_CMD_ERR_RET                 32
 
 #define mfc_read(dev, offset)           readl(dev->regs_base + (offset))
 #define mfc_write(dev, data, offset)    writel((data), dev->regs_base + \
@@ -177,6 +200,58 @@ struct s5p_mfc_pm {
         struct device   *device;
 };
 
+struct s5p_mfc_buf_size_v5 {
+        unsigned int h264_ctx;
+        unsigned int non_h264_ctx;
+        unsigned int dsc;
+        unsigned int shm;
+};
+
+struct s5p_mfc_buf_size_v6 {
+        unsigned int dev_ctx;
+        unsigned int h264_dec_ctx;
+        unsigned int other_dec_ctx;
+        unsigned int h264_enc_ctx;
+        unsigned int other_enc_ctx;
+};
+
+struct s5p_mfc_buf_size {
+        unsigned int fw;
+        unsigned int cpb;
+        void *priv;
+};
+
+struct s5p_mfc_buf_align {
+        unsigned int base;
+};
+
+struct s5p_mfc_variant {
+        unsigned int version;
+        unsigned int port_num;
+        struct s5p_mfc_buf_size *buf_size;
+        struct s5p_mfc_buf_align *buf_align;
+        char    *mclk_name;
+        char    *fw_name;
+};
+
+/**
+ * struct s5p_mfc_priv_buf - represents internal used buffer
+ * @alloc:              allocation-specific context for each buffer
+ *                      (videobuf2 allocator)
+ * @ofs:                offset of each buffer, will be used for MFC
+ * @virt:               kernel virtual address, only valid when the
+ *                      buffer accessed by driver
+ * @dma:                DMA address, only valid when kernel DMA API used
+ * @size:               size of the buffer
+ */
+struct s5p_mfc_priv_buf {
+        void            *alloc;
+        unsigned long   ofs;
+        void            *virt;
+        dma_addr_t      dma;
+        size_t          size;
+};
+
 /**
  * struct s5p_mfc_dev - The struct containing driver internal parameters.
  *
@@ -191,6 +266,7 @@ struct s5p_mfc_pm {
  * @dec_ctrl_handler:   control framework handler for decoding
  * @enc_ctrl_handler:   control framework handler for encoding
  * @pm:                 power management control
+ * @variant:            MFC hardware variant information
  * @num_inst:           couter of active MFC instances
  * @irqlock:            lock for operations on videobuf2 queues
  * @condlock:           lock for changing/checking if a context is ready to be
@@ -212,6 +288,10 @@ struct s5p_mfc_pm {
  * @watchdog_work:      worker for the watchdog
  * @alloc_ctx:          videobuf2 allocator contexts for two memory banks
  * @enter_suspend:      flag set when entering suspend
+ * @ctx_buf:            common context memory (MFCv6)
+ * @warn_start:         hardware error code from which warnings start
+ * @mfc_ops:            ops structure holding HW operation function pointers
+ * @mfc_cmds:           cmd structure holding HW commands function pointers
  *
  */
 struct s5p_mfc_dev {
@@ -226,6 +306,7 @@ struct s5p_mfc_dev {
         struct v4l2_ctrl_handler dec_ctrl_handler;
         struct v4l2_ctrl_handler enc_ctrl_handler;
         struct s5p_mfc_pm       pm;
+        struct s5p_mfc_variant  *variant;
         int num_inst;
         spinlock_t irqlock;     /* lock when operating on videobuf2 queues */
         spinlock_t condlock;    /* lock when changing/checking if a context is
@@ -248,6 +329,11 @@ struct s5p_mfc_dev {
         struct work_struct watchdog_work;
         void *alloc_ctx[2];
         unsigned long enter_suspend;
+
+        struct s5p_mfc_priv_buf ctx_buf;
+        int warn_start;
+        struct s5p_mfc_hw_ops *mfc_ops;
+        struct s5p_mfc_hw_cmds *mfc_cmds;
 };
 
 /**
@@ -262,7 +348,6 @@ struct s5p_mfc_h264_enc_params {
         u8 max_ref_pic;
         u8 num_ref_pic_4p;
         int _8x8_transform;
-        int rc_mb;
         int rc_mb_dark;
         int rc_mb_smooth;
         int rc_mb_static;
@@ -281,6 +366,23 @@ struct s5p_mfc_h264_enc_params {
         enum v4l2_mpeg_video_h264_level level_v4l2;
         int level;
         u16 cpb_size;
+        int interlace;
+        u8 hier_qp;
+        u8 hier_qp_type;
+        u8 hier_qp_layer;
+        u8 hier_qp_layer_qp[7];
+        u8 sei_frame_packing;
+        u8 sei_fp_curr_frame_0;
+        u8 sei_fp_arrangement_type;
+
+        u8 fmo;
+        u8 fmo_map_type;
+        u8 fmo_slice_grp;
+        u8 fmo_chg_dir;
+        u32 fmo_chg_rate;
+        u32 fmo_run_len[4];
+        u8 aso;
+        u32 aso_slice_order[8];
 };
 
 /**
@@ -319,9 +421,11 @@ struct s5p_mfc_enc_params {
         u8 pad_cb;
         u8 pad_cr;
         int rc_frame;
+        int rc_mb;
         u32 rc_bitrate;
         u16 rc_reaction_coeff;
         u16 vbv_size;
+        u32 vbv_delay;
 
         enum v4l2_mpeg_video_header_mode seq_hdr_mode;
         enum v4l2_mpeg_mfc51_video_frame_skip_mode frame_skip_mode;
@@ -330,7 +434,6 @@ struct s5p_mfc_enc_params {
         u8 num_b_frame;
         u32 rc_framerate_num;
         u32 rc_framerate_denom;
-        int interlace;
 
         union {
                 struct s5p_mfc_h264_enc_params h264;
@@ -388,6 +491,8 @@ struct s5p_mfc_codec_ops {
  *                      decoding buffer
  * @dpb_flush_flag:     flag used to indicate that a DPB buffers are being
  *                      flushed
+ * @head_processed:     flag mentioning whether the header data is processed
+ *                      completely or not
  * @bank1_buf:          handle to memory allocated for temporary buffers from
  *                      memory bank 1
  * @bank1_phys:         address of the temporary buffers from memory bank 1
@@ -412,19 +517,20 @@ struct s5p_mfc_codec_ops {
  * @display_delay_enable:       display delay for H264 enable flag
  * @after_packed_pb:    flag used to track buffer when stream is in
  *                      Packed PB format
+ * @sei_fp_parse:       enable/disable parsing of frame packing SEI information
  * @dpb_count:          count of the DPB buffers required by MFC hw
  * @total_dpb_count:    count of DPB buffers with additional buffers
  *                      requested by the application
- * @ctx_buf:            handle to the memory associated with this context
- * @ctx_phys:           address of the memory associated with this context
- * @ctx_size:           size of the memory associated with this context
- * @desc_buf:           description buffer for decoding handle
- * @desc_phys:          description buffer for decoding address
- * @shm_alloc:          handle for the shared memory buffer
- * @shm:                virtual address for the shared memory buffer
- * @shm_ofs:            address offset for shared memory
+ * @ctx:                context buffer information
+ * @dsc:                descriptor buffer information
+ * @shm:                shared memory buffer information
+ * @mv_count:           number of MV buffers allocated for decoding
  * @enc_params:         encoding parameters for MFC
  * @enc_dst_buf_size:   size of the buffers for encoder output
+ * @luma_dpb_size:      dpb buffer size for luma
+ * @chroma_dpb_size:    dpb buffer size for chroma
+ * @me_buffer_size:     size of the motion estimation buffer
+ * @tmv_buffer_size:    size of temporal predictor motion vector buffer
  * @frame_type:         used to force the type of the next encoded frame
  * @ref_queue:          list of the reference buffers for encoding
  * @ref_queue_cnt:      number of the buffers in the reference list
@@ -473,6 +579,7 @@ struct s5p_mfc_ctx {
         unsigned long consumed_stream;
 
         unsigned int dpb_flush_flag;
+        unsigned int head_processed;
 
         /* Buffers */
         void *bank1_buf;
@@ -502,37 +609,41 @@ struct s5p_mfc_ctx {
         int display_delay;
         int display_delay_enable;
         int after_packed_pb;
+        int sei_fp_parse;
 
         int dpb_count;
         int total_dpb_count;
-
+        int mv_count;
         /* Buffers */
-        void *ctx_buf;
-        size_t ctx_phys;
-        size_t ctx_ofs;
-        size_t ctx_size;
-
-        void *desc_buf;
-        size_t desc_phys;
-
-
-        void *shm_alloc;
-        void *shm;
-        size_t shm_ofs;
+        struct s5p_mfc_priv_buf ctx;
+        struct s5p_mfc_priv_buf dsc;
+        struct s5p_mfc_priv_buf shm;
 
         struct s5p_mfc_enc_params enc_params;
 
         size_t enc_dst_buf_size;
+        size_t luma_dpb_size;
+        size_t chroma_dpb_size;
+        size_t me_buffer_size;
+        size_t tmv_buffer_size;
 
         enum v4l2_mpeg_mfc51_video_force_frame_type force_frame_type;
 
         struct list_head ref_queue;
         unsigned int ref_queue_cnt;
 
+        enum v4l2_mpeg_video_multi_slice_mode slice_mode;
+        union {
+                unsigned int mb;
+                unsigned int bits;
+        } slice_size;
+
         struct s5p_mfc_codec_ops *c_ops;
 
         struct v4l2_ctrl *ctrls[MFC_MAX_CTRLS];
         struct v4l2_ctrl_handler ctrl_handler;
+        unsigned int frame_tag;
+        size_t scratch_buf_size;
 };
 
 /*
@@ -565,6 +676,9 @@ struct mfc_control {
         __u8                    is_volatile;
 };
 
+/* Macro for making hardware specific calls */
+#define s5p_mfc_hw_call(f, op, args...) \
+        ((f && f->op) ? f->op(args) : -ENODEV)
 
 #define fh_to_ctx(__fh) container_of(__fh, struct s5p_mfc_ctx, fh)
 #define ctrl_to_ctx(__ctrl) \
@@ -575,4 +689,9 @@ void set_work_bit(struct s5p_mfc_ctx *ctx);
 void clear_work_bit_irqsave(struct s5p_mfc_ctx *ctx);
 void set_work_bit_irqsave(struct s5p_mfc_ctx *ctx);
 
+#define HAS_PORTNUM(dev)        (dev ? (dev->variant ? \
+                                (dev->variant->port_num ? 1 : 0) : 0) : 0)
+#define IS_TWOPORT(dev)         (dev->variant->port_num == 2 ? 1 : 0)
+#define IS_MFCV6(dev)           (dev->variant->version >= 0x60 ? 1 : 0)
+
 #endif /* S5P_MFC_COMMON_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
index 0deba6bc687c..585b7b0ed8ec 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
@@ -15,11 +15,11 @@
 #include <linux/firmware.h>
 #include <linux/jiffies.h>
 #include <linux/sched.h>
-#include "regs-mfc.h"
 #include "s5p_mfc_cmd.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
 #include "s5p_mfc_pm.h"
 
 static void *s5p_mfc_bitproc_buf;
@@ -37,13 +37,19 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
         /* Firmare has to be present as a separate file or compiled
          * into kernel. */
         mfc_debug_enter();
+
         err = request_firmware((const struct firmware **)&fw_blob,
-                                     "s5p-mfc.fw", dev->v4l2_dev.dev);
+                                     dev->variant->fw_name, dev->v4l2_dev.dev);
         if (err != 0) {
                 mfc_err("Firmware is not present in the /lib/firmware directory nor compiled in kernel\n");
                 return -EINVAL;
         }
-        dev->fw_size = ALIGN(fw_blob->size, FIRMWARE_ALIGN);
+        dev->fw_size = dev->variant->buf_size->fw;
+        if (fw_blob->size > dev->fw_size) {
+                mfc_err("MFC firmware is too big to be loaded\n");
+                release_firmware(fw_blob);
+                return -ENOMEM;
+        }
         if (s5p_mfc_bitproc_buf) {
                 mfc_err("Attempting to allocate firmware when it seems that it is already loaded\n");
                 release_firmware(fw_blob);
@@ -77,32 +83,37 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
                 return -EIO;
         }
         dev->bank1 = s5p_mfc_bitproc_phys;
-        b_base = vb2_dma_contig_memops.alloc(
-                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], 1 << MFC_BASE_ALIGN_ORDER);
-        if (IS_ERR(b_base)) {
-                vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
-                s5p_mfc_bitproc_phys = 0;
-                s5p_mfc_bitproc_buf = NULL;
-                mfc_err("Allocating bank2 base failed\n");
-        release_firmware(fw_blob);
-                return -ENOMEM;
-        }
-        bank2_base_phys = s5p_mfc_mem_cookie(
-                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], b_base);
-        vb2_dma_contig_memops.put(b_base);
-        if (bank2_base_phys & ((1 << MFC_BASE_ALIGN_ORDER) - 1)) {
-                mfc_err("The base memory for bank 2 is not aligned to 128KB\n");
-                vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
-                s5p_mfc_bitproc_phys = 0;
-                s5p_mfc_bitproc_buf = NULL;
-                release_firmware(fw_blob);
-                return -EIO;
+        if (HAS_PORTNUM(dev) && IS_TWOPORT(dev)) {
+                b_base = vb2_dma_contig_memops.alloc(
+                        dev->alloc_ctx[MFC_BANK2_ALLOC_CTX],
+                        1 << MFC_BASE_ALIGN_ORDER);
+                if (IS_ERR(b_base)) {
+                        vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
+                        s5p_mfc_bitproc_phys = 0;
+                        s5p_mfc_bitproc_buf = NULL;
+                        mfc_err("Allocating bank2 base failed\n");
+                        release_firmware(fw_blob);
+                        return -ENOMEM;
+                }
+                bank2_base_phys = s5p_mfc_mem_cookie(
+                        dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], b_base);
+                vb2_dma_contig_memops.put(b_base);
+                if (bank2_base_phys & ((1 << MFC_BASE_ALIGN_ORDER) - 1)) {
+                        mfc_err("The base memory for bank 2 is not aligned to 128KB\n");
+                        vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
+                        s5p_mfc_bitproc_phys = 0;
+                        s5p_mfc_bitproc_buf = NULL;
+                        release_firmware(fw_blob);
+                        return -EIO;
+                }
+                /* Valid buffers passed to MFC encoder with LAST_FRAME command
+                 * should not have address of bank2 - MFC will treat it as a null frame.
+                 * To avoid such situation we set bank2 address below the pool address.
+                 */
+                dev->bank2 = bank2_base_phys - (1 << MFC_BASE_ALIGN_ORDER);
+        } else {
+                dev->bank2 = dev->bank1;
         }
-        /* Valid buffers passed to MFC encoder with LAST_FRAME command
-         * should not have address of bank2 - MFC will treat it as a null frame.
-         * To avoid such situation we set bank2 address below the pool address.
-         */
-        dev->bank2 = bank2_base_phys - (1 << MFC_BASE_ALIGN_ORDER);
         memcpy(s5p_mfc_bitproc_virt, fw_blob->data, fw_blob->size);
         wmb();
         release_firmware(fw_blob);
@@ -119,8 +130,9 @@ int s5p_mfc_reload_firmware(struct s5p_mfc_dev *dev)
         /* Firmare has to be present as a separate file or compiled
          * into kernel. */
         mfc_debug_enter();
+
         err = request_firmware((const struct firmware **)&fw_blob,
-                                     "s5p-mfc.fw", dev->v4l2_dev.dev);
+                                     dev->variant->fw_name, dev->v4l2_dev.dev);
         if (err != 0) {
                 mfc_err("Firmware is not present in the /lib/firmware directory nor compiled in kernel\n");
                 return -EINVAL;
@@ -161,46 +173,81 @@ int s5p_mfc_reset(struct s5p_mfc_dev *dev)
 {
         unsigned int mc_status;
         unsigned long timeout;
+        int i;
 
         mfc_debug_enter();
-        /* Stop procedure */
-        /*  reset RISC */
-        mfc_write(dev, 0x3f6, S5P_FIMV_SW_RESET);
-        /*  All reset except for MC */
-        mfc_write(dev, 0x3e2, S5P_FIMV_SW_RESET);
-        mdelay(10);
-
-        timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
-        /* Check MC status */
-        do {
-                if (time_after(jiffies, timeout)) {
-                        mfc_err("Timeout while resetting MFC\n");
-                        return -EIO;
-                }
 
-                mc_status = mfc_read(dev, S5P_FIMV_MC_STATUS);
+        if (IS_MFCV6(dev)) {
+                /* Reset IP */
+                /*  except RISC, reset */
+                mfc_write(dev, 0xFEE, S5P_FIMV_MFC_RESET_V6);
+                /*  reset release */
+                mfc_write(dev, 0x0, S5P_FIMV_MFC_RESET_V6);
+
+                /* Zero Initialization of MFC registers */
+                mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD_V6);
+                mfc_write(dev, 0, S5P_FIMV_HOST2RISC_CMD_V6);
+                mfc_write(dev, 0, S5P_FIMV_FW_VERSION_V6);
+
+                for (i = 0; i < S5P_FIMV_REG_CLEAR_COUNT_V6; i++)
+                        mfc_write(dev, 0, S5P_FIMV_REG_CLEAR_BEGIN_V6 + (i*4));
 
-        } while (mc_status & 0x3);
+                /* Reset */
+                mfc_write(dev, 0, S5P_FIMV_RISC_ON_V6);
+                mfc_write(dev, 0x1FFF, S5P_FIMV_MFC_RESET_V6);
+                mfc_write(dev, 0, S5P_FIMV_MFC_RESET_V6);
+        } else {
+                /* Stop procedure */
+                /*  reset RISC */
+                mfc_write(dev, 0x3f6, S5P_FIMV_SW_RESET);
+                /*  All reset except for MC */
+                mfc_write(dev, 0x3e2, S5P_FIMV_SW_RESET);
+                mdelay(10);
+
+                timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
+                /* Check MC status */
+                do {
+                        if (time_after(jiffies, timeout)) {
+                                mfc_err("Timeout while resetting MFC\n");
+                                return -EIO;
+                        }
+
+                        mc_status = mfc_read(dev, S5P_FIMV_MC_STATUS);
+
+                } while (mc_status & 0x3);
+
+                mfc_write(dev, 0x0, S5P_FIMV_SW_RESET);
+                mfc_write(dev, 0x3fe, S5P_FIMV_SW_RESET);
+        }
 
-        mfc_write(dev, 0x0, S5P_FIMV_SW_RESET);
-        mfc_write(dev, 0x3fe, S5P_FIMV_SW_RESET);
         mfc_debug_leave();
         return 0;
 }
 
 static inline void s5p_mfc_init_memctrl(struct s5p_mfc_dev *dev)
 {
-        mfc_write(dev, dev->bank1, S5P_FIMV_MC_DRAMBASE_ADR_A);
-        mfc_write(dev, dev->bank2, S5P_FIMV_MC_DRAMBASE_ADR_B);
-        mfc_debug(2, "Bank1: %08x, Bank2: %08x\n", dev->bank1, dev->bank2);
+        if (IS_MFCV6(dev)) {
+                mfc_write(dev, dev->bank1, S5P_FIMV_RISC_BASE_ADDRESS_V6);
+                mfc_debug(2, "Base Address : %08x\n", dev->bank1);
+        } else {
+                mfc_write(dev, dev->bank1, S5P_FIMV_MC_DRAMBASE_ADR_A);
+                mfc_write(dev, dev->bank2, S5P_FIMV_MC_DRAMBASE_ADR_B);
+                mfc_debug(2, "Bank1: %08x, Bank2: %08x\n",
+                                dev->bank1, dev->bank2);
+        }
 }
 
 static inline void s5p_mfc_clear_cmds(struct s5p_mfc_dev *dev)
 {
-        mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH0_INST_ID);
-        mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH1_INST_ID);
-        mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
-        mfc_write(dev, 0, S5P_FIMV_HOST2RISC_CMD);
+        if (IS_MFCV6(dev)) {
+                /* Zero initialization should be done before RESET.
+                 * Nothing to do here. */
+        } else {
+                mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH0_INST_ID);
+                mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH1_INST_ID);
+                mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
+                mfc_write(dev, 0, S5P_FIMV_HOST2RISC_CMD);
+        }
 }
 
 /* Initialize hardware */
@@ -228,9 +275,12 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
         s5p_mfc_clear_cmds(dev);
         /* 3. Release reset signal to the RISC */
         s5p_mfc_clean_dev_int_flags(dev);
-        mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
+        if (IS_MFCV6(dev))
+                mfc_write(dev, 0x1, S5P_FIMV_RISC_ON_V6);
+        else
+                mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
         mfc_debug(2, "Will now wait for completion of firmware transfer\n");
-        if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_FW_STATUS_RET)) {
+        if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_FW_STATUS_RET)) {
                 mfc_err("Failed to load firmware\n");
                 s5p_mfc_reset(dev);
                 s5p_mfc_clock_off();
@@ -238,7 +288,7 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
         }
         s5p_mfc_clean_dev_int_flags(dev);
         /* 4. Initialize firmware */
-        ret = s5p_mfc_sys_init_cmd(dev);
+        ret = s5p_mfc_hw_call(dev->mfc_cmds, sys_init_cmd, dev);
         if (ret) {
                 mfc_err("Failed to send command to MFC - timeout\n");
                 s5p_mfc_reset(dev);
@@ -246,7 +296,7 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
                 return ret;
         }
         mfc_debug(2, "Ok, now will write a command to init the system\n");
-        if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_SYS_INIT_RET)) {
+        if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_SYS_INIT_RET)) {
                 mfc_err("Failed to load firmware\n");
                 s5p_mfc_reset(dev);
                 s5p_mfc_clock_off();
@@ -254,7 +304,7 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
         }
         dev->int_cond = 0;
         if (dev->int_err != 0 || dev->int_type !=
-                                        S5P_FIMV_R2H_CMD_SYS_INIT_RET) {
+                                        S5P_MFC_R2H_CMD_SYS_INIT_RET) {
                 /* Failure. */
                 mfc_err("Failed to init firmware - error: %d int: %d\n",
                                                 dev->int_err, dev->int_type);
@@ -262,7 +312,11 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
                 s5p_mfc_clock_off();
                 return -EIO;
         }
-        ver = mfc_read(dev, S5P_FIMV_FW_VERSION);
+        if (IS_MFCV6(dev))
+                ver = mfc_read(dev, S5P_FIMV_FW_VERSION_V6);
+        else
+                ver = mfc_read(dev, S5P_FIMV_FW_VERSION);
+
         mfc_debug(2, "MFC F/W version : %02xyy, %02xmm, %02xdd\n",
                 (ver >> 16) & 0xFF, (ver >> 8) & 0xFF, ver & 0xFF);
         s5p_mfc_clock_off();
@@ -271,6 +325,17 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
 }
 
 
+/* Deinitialize hardware */
+void s5p_mfc_deinit_hw(struct s5p_mfc_dev *dev)
+{
+        s5p_mfc_clock_on();
+
+        s5p_mfc_reset(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, release_dev_context_buffer, dev);
+
+        s5p_mfc_clock_off();
+}
+
 int s5p_mfc_sleep(struct s5p_mfc_dev *dev)
 {
         int ret;
@@ -278,19 +343,19 @@ int s5p_mfc_sleep(struct s5p_mfc_dev *dev)
         mfc_debug_enter();
         s5p_mfc_clock_on();
         s5p_mfc_clean_dev_int_flags(dev);
-        ret = s5p_mfc_sleep_cmd(dev);
+        ret = s5p_mfc_hw_call(dev->mfc_cmds, sleep_cmd, dev);
         if (ret) {
                 mfc_err("Failed to send command to MFC - timeout\n");
                 return ret;
         }
-        if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_SLEEP_RET)) {
+        if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_SLEEP_RET)) {
                 mfc_err("Failed to sleep\n");
                 return -EIO;
         }
         s5p_mfc_clock_off();
         dev->int_cond = 0;
         if (dev->int_err != 0 || dev->int_type !=
-                                                S5P_FIMV_R2H_CMD_SLEEP_RET) {
+                                                S5P_MFC_R2H_CMD_SLEEP_RET) {
                 /* Failure. */
                 mfc_err("Failed to sleep - error: %d int: %d\n", dev->int_err,
                                                                 dev->int_type);
@@ -320,22 +385,25 @@ int s5p_mfc_wakeup(struct s5p_mfc_dev *dev)
         s5p_mfc_clear_cmds(dev);
         s5p_mfc_clean_dev_int_flags(dev);
         /* 3. Initialize firmware */
-        ret = s5p_mfc_wakeup_cmd(dev);
+        ret = s5p_mfc_hw_call(dev->mfc_cmds, wakeup_cmd, dev);
         if (ret) {
                 mfc_err("Failed to send command to MFC - timeout\n");
                 return ret;
         }
         /* 4. Release reset signal to the RISC */
-        mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
+        if (IS_MFCV6(dev))
+                mfc_write(dev, 0x1, S5P_FIMV_RISC_ON_V6);
+        else
+                mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
         mfc_debug(2, "Ok, now will write a command to wakeup the system\n");
-        if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_WAKEUP_RET)) {
+        if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_WAKEUP_RET)) {
                 mfc_err("Failed to load firmware\n");
                 return -EIO;
         }
         s5p_mfc_clock_off();
         dev->int_cond = 0;
         if (dev->int_err != 0 || dev->int_type !=
-                                                S5P_FIMV_R2H_CMD_WAKEUP_RET) {
+                                                S5P_MFC_R2H_CMD_WAKEUP_RET) {
                 /* Failure. */
                 mfc_err("Failed to wakeup - error: %d int: %d\n", dev->int_err,
                                                                 dev->int_type);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
index e1e0c544b6a2..90aa9b9886d5 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
@@ -20,6 +20,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev);
 int s5p_mfc_reload_firmware(struct s5p_mfc_dev *dev);
 
 int s5p_mfc_init_hw(struct s5p_mfc_dev *dev);
+void s5p_mfc_deinit_hw(struct s5p_mfc_dev *dev);
 
 int s5p_mfc_sleep(struct s5p_mfc_dev *dev);
 int s5p_mfc_wakeup(struct s5p_mfc_dev *dev);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
index 6ee21bb71398..eb6a70b0f821 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
@@ -23,85 +23,114 @@
 #include <linux/workqueue.h>
 #include <media/v4l2-ctrls.h>
 #include <media/videobuf2-core.h>
-#include "regs-mfc.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_dec.h"
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
 #include "s5p_mfc_pm.h"
-#include "s5p_mfc_shm.h"
+
+#define DEF_SRC_FMT_DEC V4L2_PIX_FMT_H264
+#define DEF_DST_FMT_DEC V4L2_PIX_FMT_NV12MT_16X16
 
 static struct s5p_mfc_fmt formats[] = {
         {
+                .name           = "4:2:0 2 Planes 16x16 Tiles",
+                .fourcc         = V4L2_PIX_FMT_NV12MT_16X16,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
+        },
+        {
                 .name           = "4:2:0 2 Planes 64x32 Tiles",
                 .fourcc         = V4L2_PIX_FMT_NV12MT,
-                .codec_mode     = S5P_FIMV_CODEC_NONE,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
                 .type           = MFC_FMT_RAW,
                 .num_planes     = 2,
-         },
+        },
         {
-                .name = "4:2:0 2 Planes",
-                .fourcc = V4L2_PIX_FMT_NV12M,
-                .codec_mode = S5P_FIMV_CODEC_NONE,
-                .type = MFC_FMT_RAW,
-                .num_planes = 2,
+                .name           = "4:2:0 2 Planes Y/CbCr",
+                .fourcc         = V4L2_PIX_FMT_NV12M,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
+        },
+        {
+                .name           = "4:2:0 2 Planes Y/CrCb",
+                .fourcc         = V4L2_PIX_FMT_NV21M,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
         },
         {
-                .name = "H264 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_H264,
-                .codec_mode = S5P_FIMV_CODEC_H264_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "H264 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_H264,
+                .codec_mode     = S5P_MFC_CODEC_H264_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "H263 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_H263,
-                .codec_mode = S5P_FIMV_CODEC_H263_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "H264/MVC Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_H264_MVC,
+                .codec_mode     = S5P_MFC_CODEC_H264_MVC_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "MPEG1 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_MPEG1,
-                .codec_mode = S5P_FIMV_CODEC_MPEG2_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "H263 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_H263,
+                .codec_mode     = S5P_MFC_CODEC_H263_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "MPEG2 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_MPEG2,
-                .codec_mode = S5P_FIMV_CODEC_MPEG2_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "MPEG1 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_MPEG1,
+                .codec_mode     = S5P_MFC_CODEC_MPEG2_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "MPEG4 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_MPEG4,
-                .codec_mode = S5P_FIMV_CODEC_MPEG4_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "MPEG2 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_MPEG2,
+                .codec_mode     = S5P_MFC_CODEC_MPEG2_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "XviD Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_XVID,
-                .codec_mode = S5P_FIMV_CODEC_MPEG4_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "MPEG4 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_MPEG4,
+                .codec_mode     = S5P_MFC_CODEC_MPEG4_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "VC1 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_VC1_ANNEX_G,
-                .codec_mode = S5P_FIMV_CODEC_VC1_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "XviD Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_XVID,
+                .codec_mode     = S5P_MFC_CODEC_MPEG4_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
         {
-                .name = "VC1 RCV Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_VC1_ANNEX_L,
-                .codec_mode = S5P_FIMV_CODEC_VC1RCV_DEC,
-                .type = MFC_FMT_DEC,
-                .num_planes = 1,
+                .name           = "VC1 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_VC1_ANNEX_G,
+                .codec_mode     = S5P_MFC_CODEC_VC1_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
+        },
+        {
+                .name           = "VC1 RCV Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_VC1_ANNEX_L,
+                .codec_mode     = S5P_MFC_CODEC_VC1RCV_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
+        },
+        {
+                .name           = "VP8 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_VP8,
+                .codec_mode     = S5P_MFC_CODEC_VP8_DEC,
+                .type           = MFC_FMT_DEC,
+                .num_planes     = 1,
         },
 };
 
@@ -297,7 +326,7 @@ static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
                 /* If the MFC is parsing the header,
                  * so wait until it is finished */
                 s5p_mfc_clean_ctx_int_flags(ctx);
-                s5p_mfc_wait_for_done_ctx(ctx, S5P_FIMV_R2H_CMD_SEQ_DONE_RET,
+                s5p_mfc_wait_for_done_ctx(ctx, S5P_MFC_R2H_CMD_SEQ_DONE_RET,
                                                                         0);
         }
         if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
@@ -342,21 +371,36 @@ static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 /* Try format */
 static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 {
+        struct s5p_mfc_dev *dev = video_drvdata(file);
         struct s5p_mfc_fmt *fmt;
 
-        if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-                mfc_err("This node supports decoding only\n");
-                return -EINVAL;
-        }
-        fmt = find_format(f, MFC_FMT_DEC);
-        if (!fmt) {
-                mfc_err("Unsupported format\n");
-                return -EINVAL;
-        }
-        if (fmt->type != MFC_FMT_DEC) {
-                mfc_err("\n");
-                return -EINVAL;
+        mfc_debug(2, "Type is %d\n", f->type);
+        if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                fmt = find_format(f, MFC_FMT_DEC);
+                if (!fmt) {
+                        mfc_err("Unsupported format for source.\n");
+                        return -EINVAL;
+                }
+                if (!IS_MFCV6(dev) && (fmt->fourcc == V4L2_PIX_FMT_VP8)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                }
+        } else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                fmt = find_format(f, MFC_FMT_RAW);
+                if (!fmt) {
+                        mfc_err("Unsupported format for destination.\n");
+                        return -EINVAL;
+                }
+                if (IS_MFCV6(dev) && (fmt->fourcc == V4L2_PIX_FMT_NV12MT)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                } else if (!IS_MFCV6(dev) &&
+                                (fmt->fourcc != V4L2_PIX_FMT_NV12MT)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                }
         }
+
         return 0;
 }
 
@@ -379,8 +423,29 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
                 ret = -EBUSY;
                 goto out;
         }
+        if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+                fmt = find_format(f, MFC_FMT_RAW);
+                if (!fmt) {
+                        mfc_err("Unsupported format for source.\n");
+                        return -EINVAL;
+                }
+                if (!IS_MFCV6(dev) && (fmt->fourcc != V4L2_PIX_FMT_NV12MT)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                } else if (IS_MFCV6(dev) &&
+                                (fmt->fourcc == V4L2_PIX_FMT_NV12MT)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                }
+                ctx->dst_fmt = fmt;
+                mfc_debug_leave();
+                return ret;
+        } else if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+                mfc_err("Wrong type error for S_FMT : %d", f->type);
+                return -EINVAL;
+        }
         fmt = find_format(f, MFC_FMT_DEC);
-        if (!fmt || fmt->codec_mode == S5P_FIMV_CODEC_NONE) {
+        if (!fmt || fmt->codec_mode == S5P_MFC_CODEC_NONE) {
                 mfc_err("Unknown codec\n");
                 ret = -EINVAL;
                 goto out;
@@ -391,6 +456,10 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
                 ret = -EINVAL;
                 goto out;
         }
+        if (!IS_MFCV6(dev) && (fmt->fourcc == V4L2_PIX_FMT_VP8)) {
+                mfc_err("Not supported format.\n");
+                return -EINVAL;
+        }
         ctx->src_fmt = fmt;
         ctx->codec_mode = fmt->codec_mode;
         mfc_debug(2, "The codec number is: %d\n", ctx->codec_mode);
@@ -476,7 +545,7 @@ static int vidioc_reqbufs(struct file *file, void *priv,
                         return -ENOMEM;
                 }
                 ctx->total_dpb_count = reqbufs->count;
-                ret = s5p_mfc_alloc_codec_buffers(ctx);
+                ret = s5p_mfc_hw_call(dev->mfc_ops, alloc_codec_buffers, ctx);
                 if (ret) {
                         mfc_err("Failed to allocate decoding buffers\n");
                         reqbufs->count = 0;
@@ -492,15 +561,16 @@ static int vidioc_reqbufs(struct file *file, void *priv,
                         reqbufs->count = 0;
                         s5p_mfc_clock_on();
                         ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
-                        s5p_mfc_release_codec_buffers(ctx);
+                        s5p_mfc_hw_call(dev->mfc_ops, release_codec_buffers,
+                                        ctx);
                         s5p_mfc_clock_off();
                         return -ENOMEM;
                 }
                 if (s5p_mfc_ctx_ready(ctx))
                         set_work_bit_irqsave(ctx);
-                s5p_mfc_try_run(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 s5p_mfc_wait_for_done_ctx(ctx,
-                                         S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET, 0);
+                                        S5P_MFC_R2H_CMD_INIT_BUFFERS_RET, 0);
         }
         return ret;
 }
@@ -582,18 +652,22 @@ static int vidioc_streamon(struct file *file, void *priv,
                         ctx->src_bufs_cnt = 0;
                         ctx->capture_state = QUEUE_FREE;
                         ctx->output_state = QUEUE_FREE;
-                        s5p_mfc_alloc_instance_buffer(ctx);
-                        s5p_mfc_alloc_dec_temp_buffers(ctx);
+                        s5p_mfc_hw_call(dev->mfc_ops, alloc_instance_buffer,
+                                        ctx);
+                        s5p_mfc_hw_call(dev->mfc_ops, alloc_dec_temp_buffers,
+                                        ctx);
                         set_work_bit_irqsave(ctx);
                         s5p_mfc_clean_ctx_int_flags(ctx);
-                        s5p_mfc_try_run(dev);
+                        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 
                         if (s5p_mfc_wait_for_done_ctx(ctx,
-                                S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET, 0)) {
+                                S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET, 0)) {
                                 /* Error or timeout */
                                 mfc_err("Error getting instance from hardware\n");
-                                s5p_mfc_release_instance_buffer(ctx);
-                                s5p_mfc_release_dec_desc_buffer(ctx);
+                                s5p_mfc_hw_call(dev->mfc_ops,
+                                                release_instance_buffer, ctx);
+                                s5p_mfc_hw_call(dev->mfc_ops,
+                                                release_dec_desc_buffer, ctx);
                                 return -EIO;
                         }
                         mfc_debug(2, "Got instance number: %d\n", ctx->inst_no);
@@ -662,7 +736,7 @@ static int s5p_mfc_dec_g_v_ctrl(struct v4l2_ctrl *ctrl)
                 /* Should wait for the header to be parsed */
                 s5p_mfc_clean_ctx_int_flags(ctx);
                 s5p_mfc_wait_for_done_ctx(ctx,
-                                S5P_FIMV_R2H_CMD_SEQ_DONE_RET, 0);
+                                S5P_MFC_R2H_CMD_SEQ_DONE_RET, 0);
                 if (ctx->state >= MFCINST_HEAD_PARSED &&
                     ctx->state < MFCINST_ABORT) {
                         ctrl->val = ctx->dpb_count;
@@ -686,6 +760,7 @@ static int vidioc_g_crop(struct file *file, void *priv,
                 struct v4l2_crop *cr)
 {
         struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
         u32 left, right, top, bottom;
 
         if (ctx->state != MFCINST_HEAD_PARSED &&
@@ -695,10 +770,10 @@ static int vidioc_g_crop(struct file *file, void *priv,
                         return -EINVAL;
                 }
         if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_H264) {
-                left = s5p_mfc_read_shm(ctx, CROP_INFO_H);
+                left = s5p_mfc_hw_call(dev->mfc_ops, get_crop_info_h, ctx);
                 right = left >> S5P_FIMV_SHARED_CROP_RIGHT_SHIFT;
                 left = left & S5P_FIMV_SHARED_CROP_LEFT_MASK;
-                top = s5p_mfc_read_shm(ctx, CROP_INFO_V);
+                top = s5p_mfc_hw_call(dev->mfc_ops, get_crop_info_v, ctx);
                 bottom = top >> S5P_FIMV_SHARED_CROP_BOTTOM_SHIFT;
                 top = top & S5P_FIMV_SHARED_CROP_TOP_MASK;
                 cr->c.left = left;
@@ -749,6 +824,7 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
                         void *allocators[])
 {
         struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
 
         /* Video output for decoding (source)
          * this can be set after getting an instance */
@@ -784,7 +860,13 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
             vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
                 psize[0] = ctx->luma_size;
                 psize[1] = ctx->chroma_size;
-                allocators[0] = ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+
+                if (IS_MFCV6(dev))
+                        allocators[0] =
+                                ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+                else
+                        allocators[0] =
+                                ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
                 allocators[1] = ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
         } else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
                    ctx->state == MFCINST_INIT) {
@@ -876,7 +958,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
         /* If context is ready then dev = work->data;schedule it to run */
         if (s5p_mfc_ctx_ready(ctx))
                 set_work_bit_irqsave(ctx);
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
         return 0;
 }
 
@@ -892,19 +974,21 @@ static int s5p_mfc_stop_streaming(struct vb2_queue *q)
                 dev->curr_ctx == ctx->num && dev->hw_lock) {
                 ctx->state = MFCINST_ABORT;
                 s5p_mfc_wait_for_done_ctx(ctx,
-                                        S5P_FIMV_R2H_CMD_FRAME_DONE_RET, 0);
+                                        S5P_MFC_R2H_CMD_FRAME_DONE_RET, 0);
                 aborted = 1;
         }
         spin_lock_irqsave(&dev->irqlock, flags);
         if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-                s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
+                                &ctx->vq_dst);
                 INIT_LIST_HEAD(&ctx->dst_queue);
                 ctx->dst_queue_cnt = 0;
                 ctx->dpb_flush_flag = 1;
                 ctx->dec_dst_flag = 0;
         }
         if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-                s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+                                &ctx->vq_src);
                 INIT_LIST_HEAD(&ctx->src_queue);
                 ctx->src_queue_cnt = 0;
         }
@@ -944,7 +1028,7 @@ static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
         }
         if (s5p_mfc_ctx_ready(ctx))
                 set_work_bit_irqsave(ctx);
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 static struct vb2_ops s5p_mfc_dec_qops = {
@@ -1028,3 +1112,13 @@ void s5p_mfc_dec_ctrls_delete(struct s5p_mfc_ctx *ctx)
                 ctx->ctrls[i] = NULL;
 }
 
+void s5p_mfc_dec_init(struct s5p_mfc_ctx *ctx)
+{
+        struct v4l2_format f;
+        f.fmt.pix_mp.pixelformat = DEF_SRC_FMT_DEC;
+        ctx->src_fmt = find_format(&f, MFC_FMT_DEC);
+        f.fmt.pix_mp.pixelformat = DEF_DST_FMT_DEC;
+        ctx->dst_fmt = find_format(&f, MFC_FMT_RAW);
+        mfc_debug(2, "Default src_fmt is %x, dest_fmt is %x\n",
+                        (unsigned int)ctx->src_fmt, (unsigned int)ctx->dst_fmt);
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
index fdf1d99a9d15..d06a7cab5eb1 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
@@ -19,5 +19,6 @@ const struct v4l2_ioctl_ops *get_dec_v4l2_ioctl_ops(void);
 struct s5p_mfc_fmt *get_dec_def_fmt(bool src);
 int s5p_mfc_dec_ctrls_setup(struct s5p_mfc_ctx *ctx);
 void s5p_mfc_dec_ctrls_delete(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_dec_init(struct s5p_mfc_ctx *ctx);
 
 #endif /* S5P_MFC_DEC_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
index 179e4db60b15..2af6d522f4ac 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
@@ -25,48 +25,64 @@
 #include <linux/workqueue.h>
 #include <media/v4l2-ctrls.h>
 #include <media/videobuf2-core.h>
-#include "regs-mfc.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_enc.h"
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
 
+#define DEF_SRC_FMT_ENC V4L2_PIX_FMT_NV12MT
+#define DEF_DST_FMT_ENC V4L2_PIX_FMT_H264
+
 static struct s5p_mfc_fmt formats[] = {
         {
-                .name = "4:2:0 2 Planes 64x32 Tiles",
-                .fourcc = V4L2_PIX_FMT_NV12MT,
-                .codec_mode = S5P_FIMV_CODEC_NONE,
-                .type = MFC_FMT_RAW,
-                .num_planes = 2,
+                .name           = "4:2:0 2 Planes 16x16 Tiles",
+                .fourcc         = V4L2_PIX_FMT_NV12MT_16X16,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
+        },
+        {
+                .name           = "4:2:0 2 Planes 64x32 Tiles",
+                .fourcc         = V4L2_PIX_FMT_NV12MT,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
+        },
+        {
+                .name           = "4:2:0 2 Planes Y/CbCr",
+                .fourcc         = V4L2_PIX_FMT_NV12M,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
         },
         {
-                .name = "4:2:0 2 Planes",
-                .fourcc = V4L2_PIX_FMT_NV12M,
-                .codec_mode = S5P_FIMV_CODEC_NONE,
-                .type = MFC_FMT_RAW,
-                .num_planes = 2,
+                .name           = "4:2:0 2 Planes Y/CrCb",
+                .fourcc         = V4L2_PIX_FMT_NV21M,
+                .codec_mode     = S5P_MFC_CODEC_NONE,
+                .type           = MFC_FMT_RAW,
+                .num_planes     = 2,
         },
         {
-                .name = "H264 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_H264,
-                .codec_mode = S5P_FIMV_CODEC_H264_ENC,
-                .type = MFC_FMT_ENC,
-                .num_planes = 1,
+                .name           = "H264 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_H264,
+                .codec_mode     = S5P_MFC_CODEC_H264_ENC,
+                .type           = MFC_FMT_ENC,
+                .num_planes     = 1,
         },
         {
-                .name = "MPEG4 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_MPEG4,
-                .codec_mode = S5P_FIMV_CODEC_MPEG4_ENC,
-                .type = MFC_FMT_ENC,
-                .num_planes = 1,
+                .name           = "MPEG4 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_MPEG4,
+                .codec_mode     = S5P_MFC_CODEC_MPEG4_ENC,
+                .type           = MFC_FMT_ENC,
+                .num_planes     = 1,
         },
         {
-                .name = "H263 Encoded Stream",
-                .fourcc = V4L2_PIX_FMT_H263,
-                .codec_mode = S5P_FIMV_CODEC_H263_ENC,
-                .type = MFC_FMT_ENC,
-                .num_planes = 1,
+                .name           = "H263 Encoded Stream",
+                .fourcc         = V4L2_PIX_FMT_H263,
+                .codec_mode     = S5P_MFC_CODEC_H263_ENC,
+                .type           = MFC_FMT_ENC,
+                .num_planes     = 1,
         },
 };
 
@@ -574,7 +590,8 @@ static int s5p_mfc_ctx_ready(struct s5p_mfc_ctx *ctx)
         if (ctx->state == MFCINST_GOT_INST && ctx->dst_queue_cnt >= 1)
                 return 1;
         /* context is ready to encode a frame */
-        if (ctx->state == MFCINST_RUNNING &&
+        if ((ctx->state == MFCINST_RUNNING ||
+                ctx->state == MFCINST_HEAD_PARSED) &&
                 ctx->src_queue_cnt >= 1 && ctx->dst_queue_cnt >= 1)
                 return 1;
         /* context is ready to encode remaining frames */
@@ -619,7 +636,8 @@ static int enc_pre_seq_start(struct s5p_mfc_ctx *ctx)
         dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
         dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
         dst_size = vb2_plane_size(dst_mb->b, 0);
-        s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
+        s5p_mfc_hw_call(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
+                        dst_size);
         spin_unlock_irqrestore(&dev->irqlock, flags);
         return 0;
 }
@@ -638,14 +656,23 @@ static int enc_post_seq_start(struct s5p_mfc_ctx *ctx)
                 list_del(&dst_mb->list);
                 ctx->dst_queue_cnt--;
                 vb2_set_plane_payload(dst_mb->b, 0,
-                                                s5p_mfc_get_enc_strm_size());
+                        s5p_mfc_hw_call(dev->mfc_ops, get_enc_strm_size, dev));
                 vb2_buffer_done(dst_mb->b, VB2_BUF_STATE_DONE);
                 spin_unlock_irqrestore(&dev->irqlock, flags);
         }
-        ctx->state = MFCINST_RUNNING;
-        if (s5p_mfc_ctx_ready(ctx))
-                set_work_bit_irqsave(ctx);
-        s5p_mfc_try_run(dev);
+        if (IS_MFCV6(dev)) {
+                ctx->state = MFCINST_HEAD_PARSED; /* for INIT_BUFFER cmd */
+        } else {
+                ctx->state = MFCINST_RUNNING;
+                if (s5p_mfc_ctx_ready(ctx))
+                        set_work_bit_irqsave(ctx);
+                s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+        }
+
+        if (IS_MFCV6(dev))
+                ctx->dpb_count = s5p_mfc_hw_call(dev->mfc_ops,
+                                get_enc_dpb_count, dev);
+
         return 0;
 }
 
@@ -662,14 +689,16 @@ static int enc_pre_frame_start(struct s5p_mfc_ctx *ctx)
         src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
         src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 0);
         src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 1);
-        s5p_mfc_set_enc_frame_buffer(ctx, src_y_addr, src_c_addr);
+        s5p_mfc_hw_call(dev->mfc_ops, set_enc_frame_buffer, ctx, src_y_addr,
+                        src_c_addr);
         spin_unlock_irqrestore(&dev->irqlock, flags);
 
         spin_lock_irqsave(&dev->irqlock, flags);
         dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
         dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
         dst_size = vb2_plane_size(dst_mb->b, 0);
-        s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
+        s5p_mfc_hw_call(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
+                        dst_size);
         spin_unlock_irqrestore(&dev->irqlock, flags);
 
         return 0;
@@ -685,15 +714,16 @@ static int enc_post_frame_start(struct s5p_mfc_ctx *ctx)
         unsigned int strm_size;
         unsigned long flags;
 
-        slice_type = s5p_mfc_get_enc_slice_type();
-        strm_size = s5p_mfc_get_enc_strm_size();
+        slice_type = s5p_mfc_hw_call(dev->mfc_ops, get_enc_slice_type, dev);
+        strm_size = s5p_mfc_hw_call(dev->mfc_ops, get_enc_strm_size, dev);
         mfc_debug(2, "Encoded slice type: %d", slice_type);
         mfc_debug(2, "Encoded stream size: %d", strm_size);
         mfc_debug(2, "Display order: %d",
                   mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT));
         spin_lock_irqsave(&dev->irqlock, flags);
         if (slice_type >= 0) {
-                s5p_mfc_get_enc_frame_buffer(ctx, &enc_y_addr, &enc_c_addr);
+                s5p_mfc_hw_call(dev->mfc_ops, get_enc_frame_buffer, ctx,
+                                &enc_y_addr, &enc_c_addr);
                 list_for_each_entry(mb_entry, &ctx->src_queue, list) {
                         mb_y_addr = vb2_dma_contig_plane_dma_addr(mb_entry->b, 0);
                         mb_c_addr = vb2_dma_contig_plane_dma_addr(mb_entry->b, 1);
@@ -939,15 +969,16 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
                 pix_fmt_mp->plane_fmt[0].bytesperline = 0;
                 ctx->dst_bufs_cnt = 0;
                 ctx->capture_state = QUEUE_FREE;
-                s5p_mfc_alloc_instance_buffer(ctx);
+                s5p_mfc_hw_call(dev->mfc_ops, alloc_instance_buffer, ctx);
                 set_work_bit_irqsave(ctx);
                 s5p_mfc_clean_ctx_int_flags(ctx);
-                s5p_mfc_try_run(dev);
+                s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 if (s5p_mfc_wait_for_done_ctx(ctx, \
-                                S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET, 1)) {
+                                S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET, 1)) {
                                 /* Error or timeout */
                         mfc_err("Error getting instance from hardware\n");
-                        s5p_mfc_release_instance_buffer(ctx);
+                        s5p_mfc_hw_call(dev->mfc_ops, release_instance_buffer,
+                                        ctx);
                         ret = -EIO;
                         goto out;
                 }
@@ -958,6 +989,17 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
                         mfc_err("failed to set output format\n");
                         return -EINVAL;
                 }
+
+                if (!IS_MFCV6(dev) &&
+                                (fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                } else if (IS_MFCV6(dev) &&
+                                (fmt->fourcc == V4L2_PIX_FMT_NV12MT)) {
+                        mfc_err("Not supported format.\n");
+                        return -EINVAL;
+                }
+
                 if (fmt->num_planes != pix_fmt_mp->num_planes) {
                         mfc_err("failed to set output format\n");
                         ret = -EINVAL;
@@ -970,45 +1012,13 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
                 mfc_debug(2, "fmt - w: %d, h: %d, ctx - w: %d, h: %d\n",
                         pix_fmt_mp->width, pix_fmt_mp->height,
                         ctx->img_width, ctx->img_height);
-                if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
-                        ctx->buf_width = ALIGN(ctx->img_width,
-                                                        S5P_FIMV_NV12M_HALIGN);
-                        ctx->luma_size = ALIGN(ctx->img_width,
-                                S5P_FIMV_NV12M_HALIGN) * ALIGN(ctx->img_height,
-                                S5P_FIMV_NV12M_LVALIGN);
-                        ctx->chroma_size = ALIGN(ctx->img_width,
-                                S5P_FIMV_NV12M_HALIGN) * ALIGN((ctx->img_height
-                                >> 1), S5P_FIMV_NV12M_CVALIGN);
-
-                        ctx->luma_size = ALIGN(ctx->luma_size,
-                                                        S5P_FIMV_NV12M_SALIGN);
-                        ctx->chroma_size = ALIGN(ctx->chroma_size,
-                                                        S5P_FIMV_NV12M_SALIGN);
-
-                        pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
-                        pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
-                        pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
-                        pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
-
-                } else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT) {
-                        ctx->buf_width = ALIGN(ctx->img_width,
-                                                        S5P_FIMV_NV12MT_HALIGN);
-                        ctx->luma_size = ALIGN(ctx->img_width,
-                                S5P_FIMV_NV12MT_HALIGN) * ALIGN(ctx->img_height,
-                                S5P_FIMV_NV12MT_VALIGN);
-                        ctx->chroma_size = ALIGN(ctx->img_width,
-                                S5P_FIMV_NV12MT_HALIGN) * ALIGN((ctx->img_height
-                                >> 1), S5P_FIMV_NV12MT_VALIGN);
-                        ctx->luma_size = ALIGN(ctx->luma_size,
-                                                        S5P_FIMV_NV12MT_SALIGN);
-                        ctx->chroma_size = ALIGN(ctx->chroma_size,
-                                                        S5P_FIMV_NV12MT_SALIGN);
-
-                        pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
-                        pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
-                        pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
-                        pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
-                }
+
+                s5p_mfc_hw_call(dev->mfc_ops, enc_calc_src_size, ctx);
+                pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
+                pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
+                pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
+                pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
+
                 ctx->src_bufs_cnt = 0;
                 ctx->output_state = QUEUE_FREE;
         } else {
@@ -1023,6 +1033,7 @@ out:
 static int vidioc_reqbufs(struct file *file, void *priv,
                                           struct v4l2_requestbuffers *reqbufs)
 {
+        struct s5p_mfc_dev *dev = video_drvdata(file);
         struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
         int ret = 0;
 
@@ -1042,12 +1053,16 @@ static int vidioc_reqbufs(struct file *file, void *priv,
                         return ret;
                 }
                 ctx->capture_state = QUEUE_BUFS_REQUESTED;
-                ret = s5p_mfc_alloc_codec_buffers(ctx);
-                if (ret) {
-                        mfc_err("Failed to allocate encoding buffers\n");
-                        reqbufs->count = 0;
-                        ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
-                        return -ENOMEM;
+
+                if (!IS_MFCV6(dev)) {
+                        ret = s5p_mfc_hw_call(ctx->dev->mfc_ops,
+                                        alloc_codec_buffers, ctx);
+                        if (ret) {
+                                mfc_err("Failed to allocate encoding buffers\n");
+                                reqbufs->count = 0;
+                                ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+                                return -ENOMEM;
+                        }
                 }
         } else if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
                 if (ctx->output_state != QUEUE_FREE) {
@@ -1310,6 +1325,13 @@ static int s5p_mfc_enc_s_ctrl(struct v4l2_ctrl *ctrl)
                         p->codec.h264.profile =
                                 S5P_FIMV_ENC_PROFILE_H264_BASELINE;
                         break;
+                case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
+                        if (IS_MFCV6(dev))
+                                p->codec.h264.profile =
+                                S5P_FIMV_ENC_PROFILE_H264_CONSTRAINED_BASELINE;
+                        else
+                                ret = -EINVAL;
+                        break;
                 default:
                         ret = -EINVAL;
                 }
@@ -1349,7 +1371,7 @@ static int s5p_mfc_enc_s_ctrl(struct v4l2_ctrl *ctrl)
                 p->codec.h264._8x8_transform = ctrl->val;
                 break;
         case V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE:
-                p->codec.h264.rc_mb = ctrl->val;
+                p->rc_mb = ctrl->val;
                 break;
         case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP:
                 p->codec.h264.rc_frame_qp = ctrl->val;
@@ -1500,7 +1522,7 @@ int vidioc_encoder_cmd(struct file *file, void *priv,
                         mfc_debug(2, "EOS: empty src queue, entering finishing state");
                         ctx->state = MFCINST_FINISHING;
                         spin_unlock_irqrestore(&dev->irqlock, flags);
-                        s5p_mfc_try_run(dev);
+                        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
                 } else {
                         mfc_debug(2, "EOS: marking last buffer of stream");
                         buf = list_entry(ctx->src_queue.prev,
@@ -1583,6 +1605,7 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
                         unsigned int psize[], void *allocators[])
 {
         struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+        struct s5p_mfc_dev *dev = ctx->dev;
 
         if (ctx->state != MFCINST_GOT_INST) {
                 mfc_err("inavlid state: %d\n", ctx->state);
@@ -1611,8 +1634,17 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
                         *buf_count = MFC_MAX_BUFFERS;
                 psize[0] = ctx->luma_size;
                 psize[1] = ctx->chroma_size;
-                allocators[0] = ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
-                allocators[1] = ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+                if (IS_MFCV6(dev)) {
+                        allocators[0] =
+                                ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+                        allocators[1] =
+                                ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+                } else {
+                        allocators[0] =
+                                ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+                        allocators[1] =
+                                ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+                }
         } else {
                 mfc_err("inavlid queue type: %d\n", vq->type);
                 return -EINVAL;
@@ -1715,7 +1747,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
         /* If context is ready then dev = work->data;schedule it to run */
         if (s5p_mfc_ctx_ready(ctx))
                 set_work_bit_irqsave(ctx);
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
         return 0;
 }
 
@@ -1729,19 +1761,21 @@ static int s5p_mfc_stop_streaming(struct vb2_queue *q)
                 ctx->state == MFCINST_RUNNING) &&
                 dev->curr_ctx == ctx->num && dev->hw_lock) {
                 ctx->state = MFCINST_ABORT;
-                s5p_mfc_wait_for_done_ctx(ctx, S5P_FIMV_R2H_CMD_FRAME_DONE_RET,
+                s5p_mfc_wait_for_done_ctx(ctx, S5P_MFC_R2H_CMD_FRAME_DONE_RET,
                                           0);
         }
         ctx->state = MFCINST_FINISHED;
         spin_lock_irqsave(&dev->irqlock, flags);
         if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-                s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
+                                &ctx->vq_dst);
                 INIT_LIST_HEAD(&ctx->dst_queue);
                 ctx->dst_queue_cnt = 0;
         }
         if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
                 cleanup_ref_queue(ctx);
-                s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+                s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+                                &ctx->vq_src);
                 INIT_LIST_HEAD(&ctx->src_queue);
                 ctx->src_queue_cnt = 0;
         }
@@ -1782,7 +1816,7 @@ static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
         }
         if (s5p_mfc_ctx_ready(ctx))
                 set_work_bit_irqsave(ctx);
-        s5p_mfc_try_run(dev);
+        s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 static struct vb2_ops s5p_mfc_enc_qops = {
@@ -1880,3 +1914,13 @@ void s5p_mfc_enc_ctrls_delete(struct s5p_mfc_ctx *ctx)
         for (i = 0; i < NUM_CTRLS; i++)
                 ctx->ctrls[i] = NULL;
 }
+
+void s5p_mfc_enc_init(struct s5p_mfc_ctx *ctx)
+{
+        struct v4l2_format f;
+        f.fmt.pix_mp.pixelformat = DEF_SRC_FMT_ENC;
+        ctx->src_fmt = find_format(&f, MFC_FMT_RAW);
+        f.fmt.pix_mp.pixelformat = DEF_DST_FMT_ENC;
+        ctx->dst_fmt = find_format(&f, MFC_FMT_ENC);
+}
+
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
index ca9fd66bd310..5118d46b3a9e 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
@@ -19,5 +19,6 @@ const struct v4l2_ioctl_ops *get_enc_v4l2_ioctl_ops(void);
 struct s5p_mfc_fmt *get_enc_def_fmt(bool src);
 int s5p_mfc_enc_ctrls_setup(struct s5p_mfc_ctx *ctx);
 void s5p_mfc_enc_ctrls_delete(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_enc_init(struct s5p_mfc_ctx *ctx);
 
 #endif /* S5P_MFC_ENC_H_  */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c b/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c
index 37860e299021..5b8f0e085e6d 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c
@@ -17,7 +17,6 @@
 #include <linux/io.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
-#include "regs-mfc.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_intr.h"
@@ -28,7 +27,7 @@ int s5p_mfc_wait_for_done_dev(struct s5p_mfc_dev *dev, int command)
 
         ret = wait_event_interruptible_timeout(dev->queue,
                 (dev->int_cond && (dev->int_type == command
-                || dev->int_type == S5P_FIMV_R2H_CMD_ERR_RET)),
+                || dev->int_type == S5P_MFC_R2H_CMD_ERR_RET)),
                 msecs_to_jiffies(MFC_INT_TIMEOUT));
         if (ret == 0) {
                 mfc_err("Interrupt (dev->int_type:%d, command:%d) timed out\n",
@@ -40,7 +39,7 @@ int s5p_mfc_wait_for_done_dev(struct s5p_mfc_dev *dev, int command)
         }
         mfc_debug(1, "Finished waiting (dev->int_type:%d, command: %d)\n",
                                                         dev->int_type, command);
-        if (dev->int_type == S5P_FIMV_R2H_CMD_ERR_RET)
+        if (dev->int_type == S5P_MFC_R2H_CMD_ERR_RET)
                 return 1;
         return 0;
 }
@@ -60,12 +59,12 @@ int s5p_mfc_wait_for_done_ctx(struct s5p_mfc_ctx *ctx,
         if (interrupt) {
                 ret = wait_event_interruptible_timeout(ctx->queue,
                                 (ctx->int_cond && (ctx->int_type == command
-                        || ctx->int_type == S5P_FIMV_R2H_CMD_ERR_RET)),
+                        || ctx->int_type == S5P_MFC_R2H_CMD_ERR_RET)),
                                         msecs_to_jiffies(MFC_INT_TIMEOUT));
         } else {
                 ret = wait_event_timeout(ctx->queue,
                                 (ctx->int_cond && (ctx->int_type == command
-                        || ctx->int_type == S5P_FIMV_R2H_CMD_ERR_RET)),
+                        || ctx->int_type == S5P_MFC_R2H_CMD_ERR_RET)),
                                         msecs_to_jiffies(MFC_INT_TIMEOUT));
         }
         if (ret == 0) {
@@ -78,7 +77,7 @@ int s5p_mfc_wait_for_done_ctx(struct s5p_mfc_ctx *ctx,
         }
         mfc_debug(1, "Finished waiting (ctx->int_type:%d, command: %d)\n",
                                                         ctx->int_type, command);
-        if (ctx->int_type == S5P_FIMV_R2H_CMD_ERR_RET)
+        if (ctx->int_type == S5P_MFC_R2H_CMD_ERR_RET)
                 return 1;
         return 0;
 }
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
index 767a51271dc2..6932e90d4065 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
@@ -1,10 +1,10 @@
 /*
- * drivers/media/platform/samsung/mfc5/s5p_mfc_opr.c
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
  *
  * Samsung MFC (Multi Function Codec - FIMV) driver
  * This file contains hw related functions.
  *
- * Kamil Debski, Copyright (c) 2011 Samsung Electronics
+ * Kamil Debski, Copyright (c) 2012 Samsung Electronics Co., Ltd.
  * http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
@@ -12,1414 +12,20 @@
  * published by the Free Software Foundation.
  */
 
-#include "regs-mfc.h"
-#include "s5p_mfc_cmd.h"
-#include "s5p_mfc_common.h"
-#include "s5p_mfc_ctrl.h"
-#include "s5p_mfc_debug.h"
-#include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
-#include "s5p_mfc_pm.h"
-#include "s5p_mfc_shm.h"
-#include <asm/cacheflush.h>
-#include <linux/delay.h>
-#include <linux/dma-mapping.h>
-#include <linux/err.h>
-#include <linux/firmware.h>
-#include <linux/io.h>
-#include <linux/jiffies.h>
-#include <linux/mm.h>
-#include <linux/sched.h>
+#include "s5p_mfc_opr_v5.h"
+#include "s5p_mfc_opr_v6.h"
 
-#define OFFSETA(x)              (((x) - dev->bank1) >> MFC_OFFSET_SHIFT)
-#define OFFSETB(x)              (((x) - dev->bank2) >> MFC_OFFSET_SHIFT)
+static struct s5p_mfc_hw_ops *s5p_mfc_ops;
 
-/* Allocate temporary buffers for decoding */
-int s5p_mfc_alloc_dec_temp_buffers(struct s5p_mfc_ctx *ctx)
+void s5p_mfc_init_hw_ops(struct s5p_mfc_dev *dev)
 {
-        void *desc_virt;
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        ctx->desc_buf = vb2_dma_contig_memops.alloc(
-                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], DESC_BUF_SIZE);
-        if (IS_ERR_VALUE((int)ctx->desc_buf)) {
-                ctx->desc_buf = NULL;
-                mfc_err("Allocating DESC buffer failed\n");
-                return -ENOMEM;
-        }
-        ctx->desc_phys = s5p_mfc_mem_cookie(
-                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->desc_buf);
-        BUG_ON(ctx->desc_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
-        desc_virt = vb2_dma_contig_memops.vaddr(ctx->desc_buf);
-        if (desc_virt == NULL) {
-                vb2_dma_contig_memops.put(ctx->desc_buf);
-                ctx->desc_phys = 0;
-                ctx->desc_buf = NULL;
-                mfc_err("Remapping DESC buffer failed\n");
-                return -ENOMEM;
-        }
-        memset(desc_virt, 0, DESC_BUF_SIZE);
-        wmb();
-        return 0;
-}
-
-/* Release temporary buffers for decoding */
-void s5p_mfc_release_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
-{
-        if (ctx->desc_phys) {
-                vb2_dma_contig_memops.put(ctx->desc_buf);
-                ctx->desc_phys = 0;
-                ctx->desc_buf = NULL;
-        }
-}
-
-/* Allocate codec buffers */
-int s5p_mfc_alloc_codec_buffers(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned int enc_ref_y_size = 0;
-        unsigned int enc_ref_c_size = 0;
-        unsigned int guard_width, guard_height;
-
-        if (ctx->type == MFCINST_DECODER) {
-                mfc_debug(2, "Luma size:%d Chroma size:%d MV size:%d\n",
-                          ctx->luma_size, ctx->chroma_size, ctx->mv_size);
-                mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
-        } else if (ctx->type == MFCINST_ENCODER) {
-                enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
-                        * ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
-                enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
-
-                if (ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC) {
-                        enc_ref_c_size = ALIGN(ctx->img_width,
-                                                S5P_FIMV_NV12MT_HALIGN)
-                                                * ALIGN(ctx->img_height >> 1,
-                                                S5P_FIMV_NV12MT_VALIGN);
-                        enc_ref_c_size = ALIGN(enc_ref_c_size,
-                                                        S5P_FIMV_NV12MT_SALIGN);
-                } else {
-                        guard_width = ALIGN(ctx->img_width + 16,
-                                                        S5P_FIMV_NV12MT_HALIGN);
-                        guard_height = ALIGN((ctx->img_height >> 1) + 4,
-                                                        S5P_FIMV_NV12MT_VALIGN);
-                        enc_ref_c_size = ALIGN(guard_width * guard_height,
-                                               S5P_FIMV_NV12MT_SALIGN);
-                }
-                mfc_debug(2, "recon luma size: %d chroma size: %d\n",
-                          enc_ref_y_size, enc_ref_c_size);
+        if (IS_MFCV6(dev)) {
+                s5p_mfc_ops = s5p_mfc_init_hw_ops_v6();
+                dev->warn_start = S5P_FIMV_ERR_WARNINGS_START_V6;
         } else {
-                return -EINVAL;
-        }
-        /* Codecs have different memory requirements */
-        switch (ctx->codec_mode) {
-        case S5P_FIMV_CODEC_H264_DEC:
-                ctx->bank1_size =
-                    ALIGN(S5P_FIMV_DEC_NB_IP_SIZE +
-                                        S5P_FIMV_DEC_VERT_NB_MV_SIZE,
-                                        S5P_FIMV_DEC_BUF_ALIGN);
-                ctx->bank2_size = ctx->total_dpb_count * ctx->mv_size;
-                break;
-        case S5P_FIMV_CODEC_MPEG4_DEC:
-                ctx->bank1_size =
-                    ALIGN(S5P_FIMV_DEC_NB_DCAC_SIZE +
-                                     S5P_FIMV_DEC_UPNB_MV_SIZE +
-                                     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
-                                     S5P_FIMV_DEC_STX_PARSER_SIZE +
-                                     S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE,
-                                     S5P_FIMV_DEC_BUF_ALIGN);
-                ctx->bank2_size = 0;
-                break;
-        case S5P_FIMV_CODEC_VC1RCV_DEC:
-        case S5P_FIMV_CODEC_VC1_DEC:
-                ctx->bank1_size =
-                    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
-                             S5P_FIMV_DEC_UPNB_MV_SIZE +
-                             S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
-                             S5P_FIMV_DEC_NB_DCAC_SIZE +
-                             3 * S5P_FIMV_DEC_VC1_BITPLANE_SIZE,
-                             S5P_FIMV_DEC_BUF_ALIGN);
-                ctx->bank2_size = 0;
-                break;
-        case S5P_FIMV_CODEC_MPEG2_DEC:
-                ctx->bank1_size = 0;
-                ctx->bank2_size = 0;
-                break;
-        case S5P_FIMV_CODEC_H263_DEC:
-                ctx->bank1_size =
-                    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
-                             S5P_FIMV_DEC_UPNB_MV_SIZE +
-                             S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
-                             S5P_FIMV_DEC_NB_DCAC_SIZE,
-                             S5P_FIMV_DEC_BUF_ALIGN);
-                ctx->bank2_size = 0;
-                break;
-        case S5P_FIMV_CODEC_H264_ENC:
-                ctx->bank1_size = (enc_ref_y_size * 2) +
-                                   S5P_FIMV_ENC_UPMV_SIZE +
-                                   S5P_FIMV_ENC_COLFLG_SIZE +
-                                   S5P_FIMV_ENC_INTRAMD_SIZE +
-                                   S5P_FIMV_ENC_NBORINFO_SIZE;
-                ctx->bank2_size = (enc_ref_y_size * 2) +
-                                   (enc_ref_c_size * 4) +
-                                   S5P_FIMV_ENC_INTRAPRED_SIZE;
-                break;
-        case S5P_FIMV_CODEC_MPEG4_ENC:
-                ctx->bank1_size = (enc_ref_y_size * 2) +
-                                   S5P_FIMV_ENC_UPMV_SIZE +
-                                   S5P_FIMV_ENC_COLFLG_SIZE +
-                                   S5P_FIMV_ENC_ACDCCOEF_SIZE;
-                ctx->bank2_size = (enc_ref_y_size * 2) +
-                                   (enc_ref_c_size * 4);
-                break;
-        case S5P_FIMV_CODEC_H263_ENC:
-                ctx->bank1_size = (enc_ref_y_size * 2) +
-                                   S5P_FIMV_ENC_UPMV_SIZE +
-                                   S5P_FIMV_ENC_ACDCCOEF_SIZE;
-                ctx->bank2_size = (enc_ref_y_size * 2) +
-                                   (enc_ref_c_size * 4);
-                break;
-        default:
-                break;
-        }
-        /* Allocate only if memory from bank 1 is necessary */
-        if (ctx->bank1_size > 0) {
-                ctx->bank1_buf = vb2_dma_contig_memops.alloc(
-                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_size);
-                if (IS_ERR(ctx->bank1_buf)) {
-                        ctx->bank1_buf = NULL;
-                        printk(KERN_ERR
-                               "Buf alloc for decoding failed (port A)\n");
-                        return -ENOMEM;
-                }
-                ctx->bank1_phys = s5p_mfc_mem_cookie(
-                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_buf);
-                BUG_ON(ctx->bank1_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
-        }
-        /* Allocate only if memory from bank 2 is necessary */
-        if (ctx->bank2_size > 0) {
-                ctx->bank2_buf = vb2_dma_contig_memops.alloc(
-                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_size);
-                if (IS_ERR(ctx->bank2_buf)) {
-                        ctx->bank2_buf = NULL;
-                        mfc_err("Buf alloc for decoding failed (port B)\n");
-                        return -ENOMEM;
-                }
-                ctx->bank2_phys = s5p_mfc_mem_cookie(
-                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_buf);
-                BUG_ON(ctx->bank2_phys & ((1 << MFC_BANK2_ALIGN_ORDER) - 1));
-        }
-        return 0;
-}
-
-/* Release buffers allocated for codec */
-void s5p_mfc_release_codec_buffers(struct s5p_mfc_ctx *ctx)
-{
-        if (ctx->bank1_buf) {
-                vb2_dma_contig_memops.put(ctx->bank1_buf);
-                ctx->bank1_buf = NULL;
-                ctx->bank1_phys = 0;
-                ctx->bank1_size = 0;
-        }
-        if (ctx->bank2_buf) {
-                vb2_dma_contig_memops.put(ctx->bank2_buf);
-                ctx->bank2_buf = NULL;
-                ctx->bank2_phys = 0;
-                ctx->bank2_size = 0;
+                s5p_mfc_ops = s5p_mfc_init_hw_ops_v5();
+                dev->warn_start = S5P_FIMV_ERR_WARNINGS_START;
         }
+        dev->mfc_ops = s5p_mfc_ops;
 }
-
-/* Allocate memory for instance data buffer */
-int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx)
-{
-        void *context_virt;
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC ||
-                ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC)
-                ctx->ctx_size = MFC_H264_CTX_BUF_SIZE;
-        else
-                ctx->ctx_size = MFC_CTX_BUF_SIZE;
-        ctx->ctx_buf = vb2_dma_contig_memops.alloc(
-                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx_size);
-        if (IS_ERR(ctx->ctx_buf)) {
-                mfc_err("Allocating context buffer failed\n");
-                ctx->ctx_phys = 0;
-                ctx->ctx_buf = NULL;
-                return -ENOMEM;
-        }
-        ctx->ctx_phys = s5p_mfc_mem_cookie(
-                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx_buf);
-        BUG_ON(ctx->ctx_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
-        ctx->ctx_ofs = OFFSETA(ctx->ctx_phys);
-        context_virt = vb2_dma_contig_memops.vaddr(ctx->ctx_buf);
-        if (context_virt == NULL) {
-                mfc_err("Remapping instance buffer failed\n");
-                vb2_dma_contig_memops.put(ctx->ctx_buf);
-                ctx->ctx_phys = 0;
-                ctx->ctx_buf = NULL;
-                return -ENOMEM;
-        }
-        /* Zero content of the allocated memory */
-        memset(context_virt, 0, ctx->ctx_size);
-        wmb();
-        if (s5p_mfc_init_shm(ctx) < 0) {
-                vb2_dma_contig_memops.put(ctx->ctx_buf);
-                ctx->ctx_phys = 0;
-                ctx->ctx_buf = NULL;
-                return -ENOMEM;
-        }
-        return 0;
-}
-
-/* Release instance buffer */
-void s5p_mfc_release_instance_buffer(struct s5p_mfc_ctx *ctx)
-{
-        if (ctx->ctx_buf) {
-                vb2_dma_contig_memops.put(ctx->ctx_buf);
-                ctx->ctx_phys = 0;
-                ctx->ctx_buf = NULL;
-        }
-        if (ctx->shm_alloc) {
-                vb2_dma_contig_memops.put(ctx->shm_alloc);
-                ctx->shm_alloc = NULL;
-                ctx->shm = NULL;
-        }
-}
-
-/* Set registers for decoding temporary buffers */
-void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        mfc_write(dev, OFFSETA(ctx->desc_phys), S5P_FIMV_SI_CH0_DESC_ADR);
-        mfc_write(dev, DESC_BUF_SIZE, S5P_FIMV_SI_CH0_DESC_SIZE);
-}
-
-/* Set registers for shared buffer */
-static void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        mfc_write(dev, ctx->shm_ofs, S5P_FIMV_SI_CH0_HOST_WR_ADR);
-}
-
-/* Set registers for decoding stream buffer */
-int s5p_mfc_set_dec_stream_buffer(struct s5p_mfc_ctx *ctx, int buf_addr,
-                  unsigned int start_num_byte, unsigned int buf_size)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        mfc_write(dev, OFFSETA(buf_addr), S5P_FIMV_SI_CH0_SB_ST_ADR);
-        mfc_write(dev, ctx->dec_src_buf_size, S5P_FIMV_SI_CH0_CPB_SIZE);
-        mfc_write(dev, buf_size, S5P_FIMV_SI_CH0_SB_FRM_SIZE);
-        s5p_mfc_write_shm(ctx, start_num_byte, START_BYTE_NUM);
-        return 0;
-}
-
-/* Set decoding frame buffer */
-int s5p_mfc_set_dec_frame_buffer(struct s5p_mfc_ctx *ctx)
-{
-        unsigned int frame_size, i;
-        unsigned int frame_size_ch, frame_size_mv;
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned int dpb;
-        size_t buf_addr1, buf_addr2;
-        int buf_size1, buf_size2;
-
-        buf_addr1 = ctx->bank1_phys;
-        buf_size1 = ctx->bank1_size;
-        buf_addr2 = ctx->bank2_phys;
-        buf_size2 = ctx->bank2_size;
-        dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
-                                                ~S5P_FIMV_DPB_COUNT_MASK;
-        mfc_write(dev, ctx->total_dpb_count | dpb,
-                                                S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
-        s5p_mfc_set_shared_buffer(ctx);
-        switch (ctx->codec_mode) {
-        case S5P_FIMV_CODEC_H264_DEC:
-                mfc_write(dev, OFFSETA(buf_addr1),
-                                                S5P_FIMV_H264_VERT_NB_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_VERT_NB_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_VERT_NB_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_NB_IP_ADR);
-                buf_addr1 += S5P_FIMV_DEC_NB_IP_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_NB_IP_SIZE;
-                break;
-        case S5P_FIMV_CODEC_MPEG4_DEC:
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_NB_DCAC_ADR);
-                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_NB_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SA_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SP_ADR);
-                buf_addr1 += S5P_FIMV_DEC_STX_PARSER_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_STX_PARSER_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_OT_LINE_ADR);
-                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-                break;
-        case S5P_FIMV_CODEC_H263_DEC:
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_OT_LINE_ADR);
-                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_NB_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_SA_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_NB_DCAC_ADR);
-                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
-                break;
-        case S5P_FIMV_CODEC_VC1_DEC:
-        case S5P_FIMV_CODEC_VC1RCV_DEC:
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_NB_DCAC_ADR);
-                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_OT_LINE_ADR);
-                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_UP_NB_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_SA_MV_ADR);
-                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE3_ADR);
-                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE2_ADR);
-                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE1_ADR);
-                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-                break;
-        case S5P_FIMV_CODEC_MPEG2_DEC:
-                break;
-        default:
-                mfc_err("Unknown codec for decoding (%x)\n",
-                        ctx->codec_mode);
-                return -EINVAL;
-                break;
-        }
-        frame_size = ctx->luma_size;
-        frame_size_ch = ctx->chroma_size;
-        frame_size_mv = ctx->mv_size;
-        mfc_debug(2, "Frm size: %d ch: %d mv: %d\n", frame_size, frame_size_ch,
-                                                                frame_size_mv);
-        for (i = 0; i < ctx->total_dpb_count; i++) {
-                /* Bank2 */
-                mfc_debug(2, "Luma %d: %x\n", i,
-                                        ctx->dst_bufs[i].cookie.raw.luma);
-                mfc_write(dev, OFFSETB(ctx->dst_bufs[i].cookie.raw.luma),
-                                                S5P_FIMV_DEC_LUMA_ADR + i * 4);
-                mfc_debug(2, "\tChroma %d: %x\n", i,
-                                        ctx->dst_bufs[i].cookie.raw.chroma);
-                mfc_write(dev, OFFSETA(ctx->dst_bufs[i].cookie.raw.chroma),
-                                               S5P_FIMV_DEC_CHROMA_ADR + i * 4);
-                if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC) {
-                        mfc_debug(2, "\tBuf2: %x, size: %d\n",
-                                                        buf_addr2, buf_size2);
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                                S5P_FIMV_H264_MV_ADR + i * 4);
-                        buf_addr2 += frame_size_mv;
-                        buf_size2 -= frame_size_mv;
-                }
-        }
-        mfc_debug(2, "Buf1: %u, buf_size1: %d\n", buf_addr1, buf_size1);
-        mfc_debug(2, "Buf 1/2 size after: %d/%d (frames %d)\n",
-                        buf_size1,  buf_size2, ctx->total_dpb_count);
-        if (buf_size1 < 0 || buf_size2 < 0) {
-                mfc_debug(2, "Not enough memory has been allocated\n");
-                return -ENOMEM;
-        }
-        s5p_mfc_write_shm(ctx, frame_size, ALLOC_LUMA_DPB_SIZE);
-        s5p_mfc_write_shm(ctx, frame_size_ch, ALLOC_CHROMA_DPB_SIZE);
-        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC)
-                s5p_mfc_write_shm(ctx, frame_size_mv, ALLOC_MV_SIZE);
-        mfc_write(dev, ((S5P_FIMV_CH_INIT_BUFS & S5P_FIMV_CH_MASK)
-                                        << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
-                                                S5P_FIMV_SI_CH0_INST_ID);
-        return 0;
-}
-
-/* Set registers for encoding stream buffer */
-int s5p_mfc_set_enc_stream_buffer(struct s5p_mfc_ctx *ctx,
-                unsigned long addr, unsigned int size)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        mfc_write(dev, OFFSETA(addr), S5P_FIMV_ENC_SI_CH0_SB_ADR);
-        mfc_write(dev, size, S5P_FIMV_ENC_SI_CH0_SB_SIZE);
-        return 0;
-}
-
-void s5p_mfc_set_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
-                unsigned long y_addr, unsigned long c_addr)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        mfc_write(dev, OFFSETB(y_addr), S5P_FIMV_ENC_SI_CH0_CUR_Y_ADR);
-        mfc_write(dev, OFFSETB(c_addr), S5P_FIMV_ENC_SI_CH0_CUR_C_ADR);
-}
-
-void s5p_mfc_get_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
-                unsigned long *y_addr, unsigned long *c_addr)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        *y_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_Y_ADDR)
-                                                        << MFC_OFFSET_SHIFT);
-        *c_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_C_ADDR)
-                                                        << MFC_OFFSET_SHIFT);
-}
-
-/* Set encoding ref & codec buffer */
-int s5p_mfc_set_enc_ref_buffer(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        size_t buf_addr1, buf_addr2;
-        size_t buf_size1, buf_size2;
-        unsigned int enc_ref_y_size, enc_ref_c_size;
-        unsigned int guard_width, guard_height;
-        int i;
-
-        buf_addr1 = ctx->bank1_phys;
-        buf_size1 = ctx->bank1_size;
-        buf_addr2 = ctx->bank2_phys;
-        buf_size2 = ctx->bank2_size;
-        enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
-                * ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
-        enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
-        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC) {
-                enc_ref_c_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
-                        * ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12MT_VALIGN);
-                enc_ref_c_size = ALIGN(enc_ref_c_size, S5P_FIMV_NV12MT_SALIGN);
-        } else {
-                guard_width = ALIGN(ctx->img_width + 16,
-                                                S5P_FIMV_NV12MT_HALIGN);
-                guard_height = ALIGN((ctx->img_height >> 1) + 4,
-                                                S5P_FIMV_NV12MT_VALIGN);
-                enc_ref_c_size = ALIGN(guard_width * guard_height,
-                                       S5P_FIMV_NV12MT_SALIGN);
-        }
-        mfc_debug(2, "buf_size1: %d, buf_size2: %d\n", buf_size1, buf_size2);
-        switch (ctx->codec_mode) {
-        case S5P_FIMV_CODEC_H264_ENC:
-                for (i = 0; i < 2; i++) {
-                        mfc_write(dev, OFFSETA(buf_addr1),
-                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
-                        buf_addr1 += enc_ref_y_size;
-                        buf_size1 -= enc_ref_y_size;
-
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
-                        buf_addr2 += enc_ref_y_size;
-                        buf_size2 -= enc_ref_y_size;
-                }
-                for (i = 0; i < 4; i++) {
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
-                        buf_addr2 += enc_ref_c_size;
-                        buf_size2 -= enc_ref_c_size;
-                }
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_UP_MV_ADR);
-                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1),
-                                        S5P_FIMV_H264_COZERO_FLAG_ADR);
-                buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1),
-                                        S5P_FIMV_H264_UP_INTRA_MD_ADR);
-                buf_addr1 += S5P_FIMV_ENC_INTRAMD_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_INTRAMD_SIZE;
-                mfc_write(dev, OFFSETB(buf_addr2),
-                                        S5P_FIMV_H264_UP_INTRA_PRED_ADR);
-                buf_addr2 += S5P_FIMV_ENC_INTRAPRED_SIZE;
-                buf_size2 -= S5P_FIMV_ENC_INTRAPRED_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1),
-                                        S5P_FIMV_H264_NBOR_INFO_ADR);
-                buf_addr1 += S5P_FIMV_ENC_NBORINFO_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_NBORINFO_SIZE;
-                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
-                        buf_size1, buf_size2);
-                break;
-        case S5P_FIMV_CODEC_MPEG4_ENC:
-                for (i = 0; i < 2; i++) {
-                        mfc_write(dev, OFFSETA(buf_addr1),
-                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
-                        buf_addr1 += enc_ref_y_size;
-                        buf_size1 -= enc_ref_y_size;
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
-                        buf_addr2 += enc_ref_y_size;
-                        buf_size2 -= enc_ref_y_size;
-                }
-                for (i = 0; i < 4; i++) {
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
-                        buf_addr2 += enc_ref_c_size;
-                        buf_size2 -= enc_ref_c_size;
-                }
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_MV_ADR);
-                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1),
-                                                S5P_FIMV_MPEG4_COZERO_FLAG_ADR);
-                buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1),
-                                                S5P_FIMV_MPEG4_ACDC_COEF_ADR);
-                buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
-                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
-                        buf_size1, buf_size2);
-                break;
-        case S5P_FIMV_CODEC_H263_ENC:
-                for (i = 0; i < 2; i++) {
-                        mfc_write(dev, OFFSETA(buf_addr1),
-                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
-                        buf_addr1 += enc_ref_y_size;
-                        buf_size1 -= enc_ref_y_size;
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
-                        buf_addr2 += enc_ref_y_size;
-                        buf_size2 -= enc_ref_y_size;
-                }
-                for (i = 0; i < 4; i++) {
-                        mfc_write(dev, OFFSETB(buf_addr2),
-                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
-                        buf_addr2 += enc_ref_c_size;
-                        buf_size2 -= enc_ref_c_size;
-                }
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_MV_ADR);
-                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
-                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_ACDC_COEF_ADR);
-                buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
-                buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
-                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
-                        buf_size1, buf_size2);
-                break;
-        default:
-                mfc_err("Unknown codec set for encoding: %d\n",
-                        ctx->codec_mode);
-                return -EINVAL;
-        }
-        return 0;
-}
-
-static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_enc_params *p = &ctx->enc_params;
-        unsigned int reg;
-        unsigned int shm;
-
-        /* width */
-        mfc_write(dev, ctx->img_width, S5P_FIMV_ENC_HSIZE_PX);
-        /* height */
-        mfc_write(dev, ctx->img_height, S5P_FIMV_ENC_VSIZE_PX);
-        /* pictype : enable, IDR period */
-        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-        reg |= (1 << 18);
-        reg &= ~(0xFFFF);
-        reg |= p->gop_size;
-        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-        mfc_write(dev, 0, S5P_FIMV_ENC_B_RECON_WRITE_ON);
-        /* multi-slice control */
-        /* multi-slice MB number or bit size */
-        mfc_write(dev, p->slice_mode, S5P_FIMV_ENC_MSLICE_CTRL);
-        if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
-                mfc_write(dev, p->slice_mb, S5P_FIMV_ENC_MSLICE_MB);
-        } else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
-                mfc_write(dev, p->slice_bit, S5P_FIMV_ENC_MSLICE_BIT);
-        } else {
-                mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_MB);
-                mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_BIT);
-        }
-        /* cyclic intra refresh */
-        mfc_write(dev, p->intra_refresh_mb, S5P_FIMV_ENC_CIR_CTRL);
-        /* memory structure cur. frame */
-        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
-                mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
-        else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
-                mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
-        /* padding control & value */
-        reg = mfc_read(dev, S5P_FIMV_ENC_PADDING_CTRL);
-        if (p->pad) {
-                /** enable */
-                reg |= (1 << 31);
-                /** cr value */
-                reg &= ~(0xFF << 16);
-                reg |= (p->pad_cr << 16);
-                /** cb value */
-                reg &= ~(0xFF << 8);
-                reg |= (p->pad_cb << 8);
-                /** y value */
-                reg &= ~(0xFF);
-                reg |= (p->pad_luma);
-        } else {
-                /** disable & all value clear */
-                reg = 0;
-        }
-        mfc_write(dev, reg, S5P_FIMV_ENC_PADDING_CTRL);
-        /* rate control config. */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
-        /** frame-level rate control */
-        reg &= ~(0x1 << 9);
-        reg |= (p->rc_frame << 9);
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
-        /* bit rate */
-        if (p->rc_frame)
-                mfc_write(dev, p->rc_bitrate,
-                        S5P_FIMV_ENC_RC_BIT_RATE);
-        else
-                mfc_write(dev, 0, S5P_FIMV_ENC_RC_BIT_RATE);
-        /* reaction coefficient */
-        if (p->rc_frame)
-                mfc_write(dev, p->rc_reaction_coeff, S5P_FIMV_ENC_RC_RPARA);
-        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-        /* seq header ctrl */
-        shm &= ~(0x1 << 3);
-        shm |= (p->seq_hdr_mode << 3);
-        /* frame skip mode */
-        shm &= ~(0x3 << 1);
-        shm |= (p->frame_skip_mode << 1);
-        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-        /* fixed target bit */
-        s5p_mfc_write_shm(ctx, p->fixed_target_bit, RC_CONTROL_CONFIG);
-        return 0;
-}
-
-static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_enc_params *p = &ctx->enc_params;
-        struct s5p_mfc_h264_enc_params *p_264 = &p->codec.h264;
-        unsigned int reg;
-        unsigned int shm;
-
-        s5p_mfc_set_enc_params(ctx);
-        /* pictype : number of B */
-        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-        /* num_b_frame - 0 ~ 2 */
-        reg &= ~(0x3 << 16);
-        reg |= (p->num_b_frame << 16);
-        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-        /* profile & level */
-        reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
-        /* level */
-        reg &= ~(0xFF << 8);
-        reg |= (p_264->level << 8);
-        /* profile - 0 ~ 2 */
-        reg &= ~(0x3F);
-        reg |= p_264->profile;
-        mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
-        /* interlace  */
-        mfc_write(dev, p->interlace, S5P_FIMV_ENC_PIC_STRUCT);
-        /* height */
-        if (p->interlace)
-                mfc_write(dev, ctx->img_height >> 1, S5P_FIMV_ENC_VSIZE_PX);
-        /* loopfilter ctrl */
-        mfc_write(dev, p_264->loop_filter_mode, S5P_FIMV_ENC_LF_CTRL);
-        /* loopfilter alpha offset */
-        if (p_264->loop_filter_alpha < 0) {
-                reg = 0x10;
-                reg |= (0xFF - p_264->loop_filter_alpha) + 1;
-        } else {
-                reg = 0x00;
-                reg |= (p_264->loop_filter_alpha & 0xF);
-        }
-        mfc_write(dev, reg, S5P_FIMV_ENC_ALPHA_OFF);
-        /* loopfilter beta offset */
-        if (p_264->loop_filter_beta < 0) {
-                reg = 0x10;
-                reg |= (0xFF - p_264->loop_filter_beta) + 1;
-        } else {
-                reg = 0x00;
-                reg |= (p_264->loop_filter_beta & 0xF);
-        }
-        mfc_write(dev, reg, S5P_FIMV_ENC_BETA_OFF);
-        /* entropy coding mode */
-        if (p_264->entropy_mode == V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC)
-                mfc_write(dev, 1, S5P_FIMV_ENC_H264_ENTROPY_MODE);
-        else
-                mfc_write(dev, 0, S5P_FIMV_ENC_H264_ENTROPY_MODE);
-        /* number of ref. picture */
-        reg = mfc_read(dev, S5P_FIMV_ENC_H264_NUM_OF_REF);
-        /* num of ref. pictures of P */
-        reg &= ~(0x3 << 5);
-        reg |= (p_264->num_ref_pic_4p << 5);
-        /* max number of ref. pictures */
-        reg &= ~(0x1F);
-        reg |= p_264->max_ref_pic;
-        mfc_write(dev, reg, S5P_FIMV_ENC_H264_NUM_OF_REF);
-        /* 8x8 transform enable */
-        mfc_write(dev, p_264->_8x8_transform, S5P_FIMV_ENC_H264_TRANS_FLAG);
-        /* rate control config. */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
-        /* macroblock level rate control */
-        reg &= ~(0x1 << 8);
-        reg |= (p_264->rc_mb << 8);
-        /* frame QP */
-        reg &= ~(0x3F);
-        reg |= p_264->rc_frame_qp;
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
-        /* frame rate */
-        if (p->rc_frame && p->rc_framerate_denom)
-                mfc_write(dev, p->rc_framerate_num * 1000
-                        / p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
-        else
-                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
-        /* max & min value of QP */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
-        /* max QP */
-        reg &= ~(0x3F << 8);
-        reg |= (p_264->rc_max_qp << 8);
-        /* min QP */
-        reg &= ~(0x3F);
-        reg |= p_264->rc_min_qp;
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
-        /* macroblock adaptive scaling features */
-        if (p_264->rc_mb) {
-                reg = mfc_read(dev, S5P_FIMV_ENC_RC_MB_CTRL);
-                /* dark region */
-                reg &= ~(0x1 << 3);
-                reg |= (p_264->rc_mb_dark << 3);
-                /* smooth region */
-                reg &= ~(0x1 << 2);
-                reg |= (p_264->rc_mb_smooth << 2);
-                /* static region */
-                reg &= ~(0x1 << 1);
-                reg |= (p_264->rc_mb_static << 1);
-                /* high activity region */
-                reg &= ~(0x1);
-                reg |= p_264->rc_mb_activity;
-                mfc_write(dev, reg, S5P_FIMV_ENC_RC_MB_CTRL);
-        }
-        if (!p->rc_frame &&
-            !p_264->rc_mb) {
-                shm = s5p_mfc_read_shm(ctx, P_B_FRAME_QP);
-                shm &= ~(0xFFF);
-                shm |= ((p_264->rc_b_frame_qp & 0x3F) << 6);
-                shm |= (p_264->rc_p_frame_qp & 0x3F);
-                s5p_mfc_write_shm(ctx, shm, P_B_FRAME_QP);
-        }
-        /* extended encoder ctrl */
-        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-        /* AR VUI control */
-        shm &= ~(0x1 << 15);
-        shm |= (p_264->vui_sar << 1);
-        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-        if (p_264->vui_sar) {
-                /* aspect ration IDC */
-                shm = s5p_mfc_read_shm(ctx, SAMPLE_ASPECT_RATIO_IDC);
-                shm &= ~(0xFF);
-                shm |= p_264->vui_sar_idc;
-                s5p_mfc_write_shm(ctx, shm, SAMPLE_ASPECT_RATIO_IDC);
-                if (p_264->vui_sar_idc == 0xFF) {
-                        /* sample  AR info */
-                        shm = s5p_mfc_read_shm(ctx, EXTENDED_SAR);
-                        shm &= ~(0xFFFFFFFF);
-                        shm |= p_264->vui_ext_sar_width << 16;
-                        shm |= p_264->vui_ext_sar_height;
-                        s5p_mfc_write_shm(ctx, shm, EXTENDED_SAR);
-                }
-        }
-        /* intra picture period for H.264 */
-        shm = s5p_mfc_read_shm(ctx, H264_I_PERIOD);
-        /* control */
-        shm &= ~(0x1 << 16);
-        shm |= (p_264->open_gop << 16);
-        /* value */
-        if (p_264->open_gop) {
-                shm &= ~(0xFFFF);
-                shm |= p_264->open_gop_size;
-        }
-        s5p_mfc_write_shm(ctx, shm, H264_I_PERIOD);
-        /* extended encoder ctrl */
-        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-        /* vbv buffer size */
-        if (p->frame_skip_mode ==
-                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-                shm &= ~(0xFFFF << 16);
-                shm |= (p_264->cpb_size << 16);
-        }
-        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-        return 0;
-}
-
-static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_enc_params *p = &ctx->enc_params;
-        struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
-        unsigned int reg;
-        unsigned int shm;
-        unsigned int framerate;
-
-        s5p_mfc_set_enc_params(ctx);
-        /* pictype : number of B */
-        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-        /* num_b_frame - 0 ~ 2 */
-        reg &= ~(0x3 << 16);
-        reg |= (p->num_b_frame << 16);
-        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-        /* profile & level */
-        reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
-        /* level */
-        reg &= ~(0xFF << 8);
-        reg |= (p_mpeg4->level << 8);
-        /* profile - 0 ~ 2 */
-        reg &= ~(0x3F);
-        reg |= p_mpeg4->profile;
-        mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
-        /* quarter_pixel */
-        mfc_write(dev, p_mpeg4->quarter_pixel, S5P_FIMV_ENC_MPEG4_QUART_PXL);
-        /* qp */
-        if (!p->rc_frame) {
-                shm = s5p_mfc_read_shm(ctx, P_B_FRAME_QP);
-                shm &= ~(0xFFF);
-                shm |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 6);
-                shm |= (p_mpeg4->rc_p_frame_qp & 0x3F);
-                s5p_mfc_write_shm(ctx, shm, P_B_FRAME_QP);
-        }
-        /* frame rate */
-        if (p->rc_frame) {
-                if (p->rc_framerate_denom > 0) {
-                        framerate = p->rc_framerate_num * 1000 /
-                                                p->rc_framerate_denom;
-                        mfc_write(dev, framerate,
-                                S5P_FIMV_ENC_RC_FRAME_RATE);
-                        shm = s5p_mfc_read_shm(ctx, RC_VOP_TIMING);
-                        shm &= ~(0xFFFFFFFF);
-                        shm |= (1 << 31);
-                        shm |= ((p->rc_framerate_num & 0x7FFF) << 16);
-                        shm |= (p->rc_framerate_denom & 0xFFFF);
-                        s5p_mfc_write_shm(ctx, shm, RC_VOP_TIMING);
-                }
-        } else {
-                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
-        }
-        /* rate control config. */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
-        /* frame QP */
-        reg &= ~(0x3F);
-        reg |= p_mpeg4->rc_frame_qp;
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
-        /* max & min value of QP */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
-        /* max QP */
-        reg &= ~(0x3F << 8);
-        reg |= (p_mpeg4->rc_max_qp << 8);
-        /* min QP */
-        reg &= ~(0x3F);
-        reg |= p_mpeg4->rc_min_qp;
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
-        /* extended encoder ctrl */
-        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-        /* vbv buffer size */
-        if (p->frame_skip_mode ==
-                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-                shm &= ~(0xFFFF << 16);
-                shm |= (p->vbv_size << 16);
-        }
-        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-        return 0;
-}
-
-static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_enc_params *p = &ctx->enc_params;
-        struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
-        unsigned int reg;
-        unsigned int shm;
-
-        s5p_mfc_set_enc_params(ctx);
-        /* qp */
-        if (!p->rc_frame) {
-                shm = s5p_mfc_read_shm(ctx, P_B_FRAME_QP);
-                shm &= ~(0xFFF);
-                shm |= (p_h263->rc_p_frame_qp & 0x3F);
-                s5p_mfc_write_shm(ctx, shm, P_B_FRAME_QP);
-        }
-        /* frame rate */
-        if (p->rc_frame && p->rc_framerate_denom)
-                mfc_write(dev, p->rc_framerate_num * 1000
-                        / p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
-        else
-                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
-        /* rate control config. */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
-        /* frame QP */
-        reg &= ~(0x3F);
-        reg |= p_h263->rc_frame_qp;
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
-        /* max & min value of QP */
-        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
-        /* max QP */
-        reg &= ~(0x3F << 8);
-        reg |= (p_h263->rc_max_qp << 8);
-        /* min QP */
-        reg &= ~(0x3F);
-        reg |= p_h263->rc_min_qp;
-        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
-        /* extended encoder ctrl */
-        shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-        /* vbv buffer size */
-        if (p->frame_skip_mode ==
-                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-                shm &= ~(0xFFFF << 16);
-                shm |= (p->vbv_size << 16);
-        }
-        s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-        return 0;
-}
-
-/* Initialize decoding */
-int s5p_mfc_init_decode(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        s5p_mfc_set_shared_buffer(ctx);
-        /* Setup loop filter, for decoding this is only valid for MPEG4 */
-        if (ctx->codec_mode == S5P_FIMV_CODEC_MPEG4_DEC)
-                mfc_write(dev, ctx->loop_filter_mpeg4, S5P_FIMV_ENC_LF_CTRL);
-        else
-                mfc_write(dev, 0, S5P_FIMV_ENC_LF_CTRL);
-        mfc_write(dev, ((ctx->slice_interface & S5P_FIMV_SLICE_INT_MASK) <<
-                S5P_FIMV_SLICE_INT_SHIFT) | (ctx->display_delay_enable <<
-                S5P_FIMV_DDELAY_ENA_SHIFT) | ((ctx->display_delay &
-                S5P_FIMV_DDELAY_VAL_MASK) << S5P_FIMV_DDELAY_VAL_SHIFT),
-                S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
-        mfc_write(dev,
-        ((S5P_FIMV_CH_SEQ_HEADER & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
-                                | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-        return 0;
-}
-
-static void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned int dpb;
-
-        if (flush)
-                dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) | (
-                        S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
-        else
-                dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
-                        ~(S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
-        mfc_write(dev, dpb, S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
-}
-
-/* Decode a single frame */
-int s5p_mfc_decode_one_frame(struct s5p_mfc_ctx *ctx,
-                                        enum s5p_mfc_decode_arg last_frame)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        mfc_write(dev, ctx->dec_dst_flag, S5P_FIMV_SI_CH0_RELEASE_BUF);
-        s5p_mfc_set_shared_buffer(ctx);
-        s5p_mfc_set_flush(ctx, ctx->dpb_flush_flag);
-        /* Issue different commands to instance basing on whether it
-         * is the last frame or not. */
-        switch (last_frame) {
-        case MFC_DEC_FRAME:
-                mfc_write(dev, ((S5P_FIMV_CH_FRAME_START & S5P_FIMV_CH_MASK) <<
-                S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-                break;
-        case MFC_DEC_LAST_FRAME:
-                mfc_write(dev, ((S5P_FIMV_CH_LAST_FRAME & S5P_FIMV_CH_MASK) <<
-                S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-                break;
-        case MFC_DEC_RES_CHANGE:
-                mfc_write(dev, ((S5P_FIMV_CH_FRAME_START_REALLOC &
-                S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
-                S5P_FIMV_SI_CH0_INST_ID);
-                break;
-        }
-        mfc_debug(2, "Decoding a usual frame\n");
-        return 0;
-}
-
-int s5p_mfc_init_encode(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC)
-                s5p_mfc_set_enc_params_h264(ctx);
-        else if (ctx->codec_mode == S5P_FIMV_CODEC_MPEG4_ENC)
-                s5p_mfc_set_enc_params_mpeg4(ctx);
-        else if (ctx->codec_mode == S5P_FIMV_CODEC_H263_ENC)
-                s5p_mfc_set_enc_params_h263(ctx);
-        else {
-                mfc_err("Unknown codec for encoding (%x)\n",
-                        ctx->codec_mode);
-                return -EINVAL;
-        }
-        s5p_mfc_set_shared_buffer(ctx);
-        mfc_write(dev, ((S5P_FIMV_CH_SEQ_HEADER << 16) & 0x70000) |
-                (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-        return 0;
-}
-
-/* Encode a single frame */
-int s5p_mfc_encode_one_frame(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        int cmd;
-        /* memory structure cur. frame */
-        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
-                mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
-        else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
-                mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
-        s5p_mfc_set_shared_buffer(ctx);
-
-        if (ctx->state == MFCINST_FINISHING)
-                cmd = S5P_FIMV_CH_LAST_FRAME;
-        else
-                cmd = S5P_FIMV_CH_FRAME_START;
-        mfc_write(dev, ((cmd & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
-                                | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-
-        return 0;
-}
-
-static int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
-{
-        unsigned long flags;
-        int new_ctx;
-        int cnt;
-
-        spin_lock_irqsave(&dev->condlock, flags);
-        new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
-        cnt = 0;
-        while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
-                new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
-                if (++cnt > MFC_NUM_CONTEXTS) {
-                        /* No contexts to run */
-                        spin_unlock_irqrestore(&dev->condlock, flags);
-                        return -EAGAIN;
-                }
-        }
-        spin_unlock_irqrestore(&dev->condlock, flags);
-        return new_ctx;
-}
-
-static void s5p_mfc_run_res_change(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-
-        s5p_mfc_set_dec_stream_buffer(ctx, 0, 0, 0);
-        dev->curr_ctx = ctx->num;
-        s5p_mfc_clean_ctx_int_flags(ctx);
-        s5p_mfc_decode_one_frame(ctx, MFC_DEC_RES_CHANGE);
-}
-
-static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        struct s5p_mfc_buf *temp_vb;
-        unsigned long flags;
-        unsigned int index;
-
-        spin_lock_irqsave(&dev->irqlock, flags);
-        /* Frames are being decoded */
-        if (list_empty(&ctx->src_queue)) {
-                mfc_debug(2, "No src buffers\n");
-                spin_unlock_irqrestore(&dev->irqlock, flags);
-                return -EAGAIN;
-        }
-        /* Get the next source buffer */
-        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
-        temp_vb->flags |= MFC_BUF_FLAG_USED;
-        s5p_mfc_set_dec_stream_buffer(ctx,
-                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0), ctx->consumed_stream,
-                                        temp_vb->b->v4l2_planes[0].bytesused);
-        spin_unlock_irqrestore(&dev->irqlock, flags);
-        index = temp_vb->b->v4l2_buf.index;
-        dev->curr_ctx = ctx->num;
-        s5p_mfc_clean_ctx_int_flags(ctx);
-        if (temp_vb->b->v4l2_planes[0].bytesused == 0) {
-                last_frame = MFC_DEC_LAST_FRAME;
-                mfc_debug(2, "Setting ctx->state to FINISHING\n");
-                ctx->state = MFCINST_FINISHING;
-        }
-        s5p_mfc_decode_one_frame(ctx, last_frame);
-        return 0;
-}
-
-static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned long flags;
-        struct s5p_mfc_buf *dst_mb;
-        struct s5p_mfc_buf *src_mb;
-        unsigned long src_y_addr, src_c_addr, dst_addr;
-        unsigned int dst_size;
-
-        spin_lock_irqsave(&dev->irqlock, flags);
-        if (list_empty(&ctx->src_queue) && ctx->state != MFCINST_FINISHING) {
-                mfc_debug(2, "no src buffers\n");
-                spin_unlock_irqrestore(&dev->irqlock, flags);
-                return -EAGAIN;
-        }
-        if (list_empty(&ctx->dst_queue)) {
-                mfc_debug(2, "no dst buffers\n");
-                spin_unlock_irqrestore(&dev->irqlock, flags);
-                return -EAGAIN;
-        }
-        if (list_empty(&ctx->src_queue)) {
-                /* send null frame */
-                s5p_mfc_set_enc_frame_buffer(ctx, dev->bank2, dev->bank2);
-                src_mb = NULL;
-        } else {
-                src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
-                                                                        list);
-                src_mb->flags |= MFC_BUF_FLAG_USED;
-                if (src_mb->b->v4l2_planes[0].bytesused == 0) {
-                        /* send null frame */
-                        s5p_mfc_set_enc_frame_buffer(ctx, dev->bank2,
-                                                                dev->bank2);
-                        ctx->state = MFCINST_FINISHING;
-                } else {
-                        src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b,
-                                                                        0);
-                        src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b,
-                                                                        1);
-                        s5p_mfc_set_enc_frame_buffer(ctx, src_y_addr,
-                                                                src_c_addr);
-                        if (src_mb->flags & MFC_BUF_FLAG_EOS)
-                                ctx->state = MFCINST_FINISHING;
-                }
-        }
-        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
-        dst_mb->flags |= MFC_BUF_FLAG_USED;
-        dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
-        dst_size = vb2_plane_size(dst_mb->b, 0);
-        s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
-        spin_unlock_irqrestore(&dev->irqlock, flags);
-        dev->curr_ctx = ctx->num;
-        s5p_mfc_clean_ctx_int_flags(ctx);
-        mfc_debug(2, "encoding buffer with index=%d state=%d",
-                        src_mb ? src_mb->b->v4l2_buf.index : -1, ctx->state);
-        s5p_mfc_encode_one_frame(ctx);
-        return 0;
-}
-
-static void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned long flags;
-        struct s5p_mfc_buf *temp_vb;
-
-        /* Initializing decoding - parsing header */
-        spin_lock_irqsave(&dev->irqlock, flags);
-        mfc_debug(2, "Preparing to init decoding\n");
-        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
-        s5p_mfc_set_dec_desc_buffer(ctx);
-        mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
-        s5p_mfc_set_dec_stream_buffer(ctx,
-                                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
-                                0, temp_vb->b->v4l2_planes[0].bytesused);
-        spin_unlock_irqrestore(&dev->irqlock, flags);
-        dev->curr_ctx = ctx->num;
-        s5p_mfc_clean_ctx_int_flags(ctx);
-        s5p_mfc_init_decode(ctx);
-}
-
-static void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned long flags;
-        struct s5p_mfc_buf *dst_mb;
-        unsigned long dst_addr;
-        unsigned int dst_size;
-
-        s5p_mfc_set_enc_ref_buffer(ctx);
-        spin_lock_irqsave(&dev->irqlock, flags);
-        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
-        dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
-        dst_size = vb2_plane_size(dst_mb->b, 0);
-        s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
-        spin_unlock_irqrestore(&dev->irqlock, flags);
-        dev->curr_ctx = ctx->num;
-        s5p_mfc_clean_ctx_int_flags(ctx);
-        s5p_mfc_init_encode(ctx);
-}
-
-static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        unsigned long flags;
-        struct s5p_mfc_buf *temp_vb;
-        int ret;
-
-        /*
-         * Header was parsed now starting processing
-         * First set the output frame buffers
-         */
-        if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
-                mfc_err("It seems that not all destionation buffers were "
-                        "mmaped\nMFC requires that all destination are mmaped "
-                        "before starting processing\n");
-                return -EAGAIN;
-        }
-        spin_lock_irqsave(&dev->irqlock, flags);
-        if (list_empty(&ctx->src_queue)) {
-                mfc_err("Header has been deallocated in the middle of"
-                        " initialization\n");
-                spin_unlock_irqrestore(&dev->irqlock, flags);
-                return -EIO;
-        }
-        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
-        mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
-        s5p_mfc_set_dec_stream_buffer(ctx,
-                                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
-                                0, temp_vb->b->v4l2_planes[0].bytesused);
-        spin_unlock_irqrestore(&dev->irqlock, flags);
-        dev->curr_ctx = ctx->num;
-        s5p_mfc_clean_ctx_int_flags(ctx);
-        ret = s5p_mfc_set_dec_frame_buffer(ctx);
-        if (ret) {
-                mfc_err("Failed to alloc frame mem\n");
-                ctx->state = MFCINST_ERROR;
-        }
-        return ret;
-}
-
-/* Try running an operation on hardware */
-void s5p_mfc_try_run(struct s5p_mfc_dev *dev)
-{
-        struct s5p_mfc_ctx *ctx;
-        int new_ctx;
-        unsigned int ret = 0;
-
-        if (test_bit(0, &dev->enter_suspend)) {
-                mfc_debug(1, "Entering suspend so do not schedule any jobs\n");
-                return;
-        }
-        /* Check whether hardware is not running */
-        if (test_and_set_bit(0, &dev->hw_lock) != 0) {
-                /* This is perfectly ok, the scheduled ctx should wait */
-                mfc_debug(1, "Couldn't lock HW\n");
-                return;
-        }
-        /* Choose the context to run */
-        new_ctx = s5p_mfc_get_new_ctx(dev);
-        if (new_ctx < 0) {
-                /* No contexts to run */
-                if (test_and_clear_bit(0, &dev->hw_lock) == 0) {
-                        mfc_err("Failed to unlock hardware\n");
-                        return;
-                }
-                mfc_debug(1, "No ctx is scheduled to be run\n");
-                return;
-        }
-        ctx = dev->ctx[new_ctx];
-        /* Got context to run in ctx */
-        /*
-         * Last frame has already been sent to MFC.
-         * Now obtaining frames from MFC buffer
-         */
-        s5p_mfc_clock_on();
-        if (ctx->type == MFCINST_DECODER) {
-                s5p_mfc_set_dec_desc_buffer(ctx);
-                switch (ctx->state) {
-                case MFCINST_FINISHING:
-                        s5p_mfc_run_dec_frame(ctx, MFC_DEC_LAST_FRAME);
-                        break;
-                case MFCINST_RUNNING:
-                        ret = s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
-                        break;
-                case MFCINST_INIT:
-                        s5p_mfc_clean_ctx_int_flags(ctx);
-                        ret = s5p_mfc_open_inst_cmd(ctx);
-                        break;
-                case MFCINST_RETURN_INST:
-                        s5p_mfc_clean_ctx_int_flags(ctx);
-                        ret = s5p_mfc_close_inst_cmd(ctx);
-                        break;
-                case MFCINST_GOT_INST:
-                        s5p_mfc_run_init_dec(ctx);
-                        break;
-                case MFCINST_HEAD_PARSED:
-                        ret = s5p_mfc_run_init_dec_buffers(ctx);
-                        mfc_debug(1, "head parsed\n");
-                        break;
-                case MFCINST_RES_CHANGE_INIT:
-                        s5p_mfc_run_res_change(ctx);
-                        break;
-                case MFCINST_RES_CHANGE_FLUSH:
-                        s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
-                        break;
-                case MFCINST_RES_CHANGE_END:
-                        mfc_debug(2, "Finished remaining frames after resolution change\n");
-                        ctx->capture_state = QUEUE_FREE;
-                        mfc_debug(2, "Will re-init the codec\n");
-                        s5p_mfc_run_init_dec(ctx);
-                        break;
-                default:
-                        ret = -EAGAIN;
-                }
-        } else if (ctx->type == MFCINST_ENCODER) {
-                switch (ctx->state) {
-                case MFCINST_FINISHING:
-                case MFCINST_RUNNING:
-                        ret = s5p_mfc_run_enc_frame(ctx);
-                        break;
-                case MFCINST_INIT:
-                        s5p_mfc_clean_ctx_int_flags(ctx);
-                        ret = s5p_mfc_open_inst_cmd(ctx);
-                        break;
-                case MFCINST_RETURN_INST:
-                        s5p_mfc_clean_ctx_int_flags(ctx);
-                        ret = s5p_mfc_close_inst_cmd(ctx);
-                        break;
-                case MFCINST_GOT_INST:
-                        s5p_mfc_run_init_enc(ctx);
-                        break;
-                default:
-                        ret = -EAGAIN;
-                }
-        } else {
-                mfc_err("Invalid context type: %d\n", ctx->type);
-                ret = -EAGAIN;
-        }
-
-        if (ret) {
-                /* Free hardware lock */
-                if (test_and_clear_bit(0, &dev->hw_lock) == 0)
-                        mfc_err("Failed to unlock hardware\n");
-
-                /* This is in deed imporant, as no operation has been
-                 * scheduled, reduce the clock count as no one will
-                 * ever do this, because no interrupt related to this try_run
-                 * will ever come from hardware. */
-                s5p_mfc_clock_off();
-        }
-}
-
-
-void s5p_mfc_cleanup_queue(struct list_head *lh, struct vb2_queue *vq)
-{
-        struct s5p_mfc_buf *b;
-        int i;
-
-        while (!list_empty(lh)) {
-                b = list_entry(lh->next, struct s5p_mfc_buf, list);
-                for (i = 0; i < b->b->num_planes; i++)
-                        vb2_set_plane_payload(b->b, i, 0);
-                vb2_buffer_done(b->b, VB2_BUF_STATE_ERROR);
-                list_del(&b->list);
-        }
-}
-
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
index 2ad3def052f8..420abecafec0 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
@@ -1,10 +1,10 @@
 /*
- * drivers/media/platform/samsung/mfc5/s5p_mfc_opr.h
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
  *
  * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
  * Contains declarations of hw related functions.
  *
- * Kamil Debski, Copyright (C) 2011 Samsung Electronics
+ * Kamil Debski, Copyright (C) 2012 Samsung Electronics Co., Ltd.
  * http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
@@ -17,77 +17,68 @@
 
 #include "s5p_mfc_common.h"
 
-int s5p_mfc_init_decode(struct s5p_mfc_ctx *ctx);
-int s5p_mfc_init_encode(struct s5p_mfc_ctx *mfc_ctx);
+struct s5p_mfc_hw_ops {
+        int (*alloc_dec_temp_buffers)(struct s5p_mfc_ctx *ctx);
+        void (*release_dec_desc_buffer)(struct s5p_mfc_ctx *ctx);
+        int (*alloc_codec_buffers)(struct s5p_mfc_ctx *ctx);
+        void (*release_codec_buffers)(struct s5p_mfc_ctx *ctx);
+        int (*alloc_instance_buffer)(struct s5p_mfc_ctx *ctx);
+        void (*release_instance_buffer)(struct s5p_mfc_ctx *ctx);
+        int (*alloc_dev_context_buffer)(struct s5p_mfc_dev *dev);
+        void (*release_dev_context_buffer)(struct s5p_mfc_dev *dev);
+        void (*dec_calc_dpb_size)(struct s5p_mfc_ctx *ctx);
+        void (*enc_calc_src_size)(struct s5p_mfc_ctx *ctx);
+        int (*set_dec_stream_buffer)(struct s5p_mfc_ctx *ctx,
+                        int buf_addr, unsigned int start_num_byte,
+                        unsigned int buf_size);
+        int (*set_dec_frame_buffer)(struct s5p_mfc_ctx *ctx);
+        int (*set_enc_stream_buffer)(struct s5p_mfc_ctx *ctx,
+                        unsigned long addr, unsigned int size);
+        void (*set_enc_frame_buffer)(struct s5p_mfc_ctx *ctx,
+                        unsigned long y_addr, unsigned long c_addr);
+        void (*get_enc_frame_buffer)(struct s5p_mfc_ctx *ctx,
+                        unsigned long *y_addr, unsigned long *c_addr);
+        int (*set_enc_ref_buffer)(struct s5p_mfc_ctx *ctx);
+        int (*init_decode)(struct s5p_mfc_ctx *ctx);
+        int (*init_encode)(struct s5p_mfc_ctx *ctx);
+        int (*encode_one_frame)(struct s5p_mfc_ctx *ctx);
+        void (*try_run)(struct s5p_mfc_dev *dev);
+        void (*cleanup_queue)(struct list_head *lh,
+                        struct vb2_queue *vq);
+        void (*clear_int_flags)(struct s5p_mfc_dev *dev);
+        void (*write_info)(struct s5p_mfc_ctx *ctx, unsigned int data,
+                        unsigned int ofs);
+        unsigned int (*read_info)(struct s5p_mfc_ctx *ctx,
+                        unsigned int ofs);
+        int (*get_dspl_y_adr)(struct s5p_mfc_dev *dev);
+        int (*get_dec_y_adr)(struct s5p_mfc_dev *dev);
+        int (*get_dspl_status)(struct s5p_mfc_dev *dev);
+        int (*get_dec_status)(struct s5p_mfc_dev *dev);
+        int (*get_dec_frame_type)(struct s5p_mfc_dev *dev);
+        int (*get_disp_frame_type)(struct s5p_mfc_ctx *ctx);
+        int (*get_consumed_stream)(struct s5p_mfc_dev *dev);
+        int (*get_int_reason)(struct s5p_mfc_dev *dev);
+        int (*get_int_err)(struct s5p_mfc_dev *dev);
+        int (*err_dec)(unsigned int err);
+        int (*err_dspl)(unsigned int err);
+        int (*get_img_width)(struct s5p_mfc_dev *dev);
+        int (*get_img_height)(struct s5p_mfc_dev *dev);
+        int (*get_dpb_count)(struct s5p_mfc_dev *dev);
+        int (*get_mv_count)(struct s5p_mfc_dev *dev);
+        int (*get_inst_no)(struct s5p_mfc_dev *dev);
+        int (*get_enc_strm_size)(struct s5p_mfc_dev *dev);
+        int (*get_enc_slice_type)(struct s5p_mfc_dev *dev);
+        int (*get_enc_dpb_count)(struct s5p_mfc_dev *dev);
+        int (*get_enc_pic_count)(struct s5p_mfc_dev *dev);
+        int (*get_sei_avail_status)(struct s5p_mfc_ctx *ctx);
+        int (*get_mvc_num_views)(struct s5p_mfc_dev *dev);
+        int (*get_mvc_view_id)(struct s5p_mfc_dev *dev);
+        unsigned int (*get_pic_type_top)(struct s5p_mfc_ctx *ctx);
+        unsigned int (*get_pic_type_bot)(struct s5p_mfc_ctx *ctx);
+        unsigned int (*get_crop_info_h)(struct s5p_mfc_ctx *ctx);
+        unsigned int (*get_crop_info_v)(struct s5p_mfc_ctx *ctx);
+};
 
-/* Decoding functions */
-int s5p_mfc_set_dec_frame_buffer(struct s5p_mfc_ctx *ctx);
-int s5p_mfc_set_dec_stream_buffer(struct s5p_mfc_ctx *ctx, int buf_addr,
-                                                  unsigned int start_num_byte,
-                                                  unsigned int buf_size);
-
-/* Encoding functions */
-void s5p_mfc_set_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
-                unsigned long y_addr, unsigned long c_addr);
-int s5p_mfc_set_enc_stream_buffer(struct s5p_mfc_ctx *ctx,
-                unsigned long addr, unsigned int size);
-void s5p_mfc_get_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
-                unsigned long *y_addr, unsigned long *c_addr);
-int s5p_mfc_set_enc_ref_buffer(struct s5p_mfc_ctx *mfc_ctx);
-
-int s5p_mfc_decode_one_frame(struct s5p_mfc_ctx *ctx,
-                                        enum s5p_mfc_decode_arg last_frame);
-int s5p_mfc_encode_one_frame(struct s5p_mfc_ctx *mfc_ctx);
-
-/* Memory allocation */
-int s5p_mfc_alloc_dec_temp_buffers(struct s5p_mfc_ctx *ctx);
-void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx);
-void s5p_mfc_release_dec_desc_buffer(struct s5p_mfc_ctx *ctx);
-
-int s5p_mfc_alloc_codec_buffers(struct s5p_mfc_ctx *ctx);
-void s5p_mfc_release_codec_buffers(struct s5p_mfc_ctx *ctx);
-
-int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx);
-void s5p_mfc_release_instance_buffer(struct s5p_mfc_ctx *ctx);
-
-void s5p_mfc_try_run(struct s5p_mfc_dev *dev);
-void s5p_mfc_cleanup_queue(struct list_head *lh, struct vb2_queue *vq);
-
-#define s5p_mfc_get_dspl_y_adr()        (readl(dev->regs_base + \
-                                        S5P_FIMV_SI_DISPLAY_Y_ADR) << \
-                                        MFC_OFFSET_SHIFT)
-#define s5p_mfc_get_dec_y_adr()         (readl(dev->regs_base + \
-                                        S5P_FIMV_SI_DECODE_Y_ADR) << \
-                                        MFC_OFFSET_SHIFT)
-#define s5p_mfc_get_dspl_status()       readl(dev->regs_base + \
-                                                S5P_FIMV_SI_DISPLAY_STATUS)
-#define s5p_mfc_get_dec_status()        readl(dev->regs_base + \
-                                                S5P_FIMV_SI_DECODE_STATUS)
-#define s5p_mfc_get_frame_type()        (readl(dev->regs_base + \
-                                                S5P_FIMV_DECODE_FRAME_TYPE) \
-                                        & S5P_FIMV_DECODE_FRAME_MASK)
-#define s5p_mfc_get_consumed_stream()   readl(dev->regs_base + \
-                                                S5P_FIMV_SI_CONSUMED_BYTES)
-#define s5p_mfc_get_int_reason()        (readl(dev->regs_base + \
-                                        S5P_FIMV_RISC2HOST_CMD) & \
-                                        S5P_FIMV_RISC2HOST_CMD_MASK)
-#define s5p_mfc_get_int_err()           readl(dev->regs_base + \
-                                                S5P_FIMV_RISC2HOST_ARG2)
-#define s5p_mfc_err_dec(x)              (((x) & S5P_FIMV_ERR_DEC_MASK) >> \
-                                                        S5P_FIMV_ERR_DEC_SHIFT)
-#define s5p_mfc_err_dspl(x)             (((x) & S5P_FIMV_ERR_DSPL_MASK) >> \
-                                                        S5P_FIMV_ERR_DSPL_SHIFT)
-#define s5p_mfc_get_img_width()         readl(dev->regs_base + \
-                                                S5P_FIMV_SI_HRESOL)
-#define s5p_mfc_get_img_height()        readl(dev->regs_base + \
-                                                S5P_FIMV_SI_VRESOL)
-#define s5p_mfc_get_dpb_count()         readl(dev->regs_base + \
-                                                S5P_FIMV_SI_BUF_NUMBER)
-#define s5p_mfc_get_inst_no()           readl(dev->regs_base + \
-                                                S5P_FIMV_RISC2HOST_ARG1)
-#define s5p_mfc_get_enc_strm_size()     readl(dev->regs_base + \
-                                                S5P_FIMV_ENC_SI_STRM_SIZE)
-#define s5p_mfc_get_enc_slice_type()    readl(dev->regs_base + \
-                                                S5P_FIMV_ENC_SI_SLICE_TYPE)
+void s5p_mfc_init_hw_ops(struct s5p_mfc_dev *dev);
 
 #endif /* S5P_MFC_OPR_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
new file mode 100644
index 000000000000..bf7d010a4107
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
@@ -0,0 +1,1794 @@
+/*
+ * drivers/media/platform/samsung/mfc5/s5p_mfc_opr_v5.c
+ *
+ * Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains hw related functions.
+ *
+ * Kamil Debski, Copyright (c) 2011 Samsung Electronics
+ * http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_ctrl.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_pm.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_opr_v5.h"
+#include <asm/cacheflush.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/firmware.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+
+#define OFFSETA(x)              (((x) - dev->bank1) >> MFC_OFFSET_SHIFT)
+#define OFFSETB(x)              (((x) - dev->bank2) >> MFC_OFFSET_SHIFT)
+
+/* Allocate temporary buffers for decoding */
+int s5p_mfc_alloc_dec_temp_buffers_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
+
+        ctx->dsc.alloc = vb2_dma_contig_memops.alloc(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX],
+                        buf_size->dsc);
+        if (IS_ERR_VALUE((int)ctx->dsc.alloc)) {
+                ctx->dsc.alloc = NULL;
+                mfc_err("Allocating DESC buffer failed\n");
+                return -ENOMEM;
+        }
+        ctx->dsc.dma = s5p_mfc_mem_cookie(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->dsc.alloc);
+        BUG_ON(ctx->dsc.dma & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+        ctx->dsc.virt = vb2_dma_contig_memops.vaddr(ctx->dsc.alloc);
+        if (ctx->dsc.virt == NULL) {
+                vb2_dma_contig_memops.put(ctx->dsc.alloc);
+                ctx->dsc.dma = 0;
+                ctx->dsc.alloc = NULL;
+                mfc_err("Remapping DESC buffer failed\n");
+                return -ENOMEM;
+        }
+        memset(ctx->dsc.virt, 0, buf_size->dsc);
+        wmb();
+        return 0;
+}
+
+/* Release temporary buffers for decoding */
+void s5p_mfc_release_dec_desc_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->dsc.dma) {
+                vb2_dma_contig_memops.put(ctx->dsc.alloc);
+                ctx->dsc.alloc = NULL;
+                ctx->dsc.dma = 0;
+        }
+}
+
+/* Allocate codec buffers */
+int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int enc_ref_y_size = 0;
+        unsigned int enc_ref_c_size = 0;
+        unsigned int guard_width, guard_height;
+
+        if (ctx->type == MFCINST_DECODER) {
+                mfc_debug(2, "Luma size:%d Chroma size:%d MV size:%d\n",
+                          ctx->luma_size, ctx->chroma_size, ctx->mv_size);
+                mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
+        } else if (ctx->type == MFCINST_ENCODER) {
+                enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                        * ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+                enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
+
+                if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC) {
+                        enc_ref_c_size = ALIGN(ctx->img_width,
+                                                S5P_FIMV_NV12MT_HALIGN)
+                                                * ALIGN(ctx->img_height >> 1,
+                                                S5P_FIMV_NV12MT_VALIGN);
+                        enc_ref_c_size = ALIGN(enc_ref_c_size,
+                                                        S5P_FIMV_NV12MT_SALIGN);
+                } else {
+                        guard_width = ALIGN(ctx->img_width + 16,
+                                                        S5P_FIMV_NV12MT_HALIGN);
+                        guard_height = ALIGN((ctx->img_height >> 1) + 4,
+                                                        S5P_FIMV_NV12MT_VALIGN);
+                        enc_ref_c_size = ALIGN(guard_width * guard_height,
+                                               S5P_FIMV_NV12MT_SALIGN);
+                }
+                mfc_debug(2, "recon luma size: %d chroma size: %d\n",
+                          enc_ref_y_size, enc_ref_c_size);
+        } else {
+                return -EINVAL;
+        }
+        /* Codecs have different memory requirements */
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_DEC:
+                ctx->bank1_size =
+                    ALIGN(S5P_FIMV_DEC_NB_IP_SIZE +
+                                        S5P_FIMV_DEC_VERT_NB_MV_SIZE,
+                                        S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->bank2_size = ctx->total_dpb_count * ctx->mv_size;
+                break;
+        case S5P_MFC_CODEC_MPEG4_DEC:
+                ctx->bank1_size =
+                    ALIGN(S5P_FIMV_DEC_NB_DCAC_SIZE +
+                                     S5P_FIMV_DEC_UPNB_MV_SIZE +
+                                     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+                                     S5P_FIMV_DEC_STX_PARSER_SIZE +
+                                     S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE,
+                                     S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->bank2_size = 0;
+                break;
+        case S5P_MFC_CODEC_VC1RCV_DEC:
+        case S5P_MFC_CODEC_VC1_DEC:
+                ctx->bank1_size =
+                    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
+                             S5P_FIMV_DEC_UPNB_MV_SIZE +
+                             S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+                             S5P_FIMV_DEC_NB_DCAC_SIZE +
+                             3 * S5P_FIMV_DEC_VC1_BITPLANE_SIZE,
+                             S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->bank2_size = 0;
+                break;
+        case S5P_MFC_CODEC_MPEG2_DEC:
+                ctx->bank1_size = 0;
+                ctx->bank2_size = 0;
+                break;
+        case S5P_MFC_CODEC_H263_DEC:
+                ctx->bank1_size =
+                    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
+                             S5P_FIMV_DEC_UPNB_MV_SIZE +
+                             S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+                             S5P_FIMV_DEC_NB_DCAC_SIZE,
+                             S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->bank2_size = 0;
+                break;
+        case S5P_MFC_CODEC_H264_ENC:
+                ctx->bank1_size = (enc_ref_y_size * 2) +
+                                   S5P_FIMV_ENC_UPMV_SIZE +
+                                   S5P_FIMV_ENC_COLFLG_SIZE +
+                                   S5P_FIMV_ENC_INTRAMD_SIZE +
+                                   S5P_FIMV_ENC_NBORINFO_SIZE;
+                ctx->bank2_size = (enc_ref_y_size * 2) +
+                                   (enc_ref_c_size * 4) +
+                                   S5P_FIMV_ENC_INTRAPRED_SIZE;
+                break;
+        case S5P_MFC_CODEC_MPEG4_ENC:
+                ctx->bank1_size = (enc_ref_y_size * 2) +
+                                   S5P_FIMV_ENC_UPMV_SIZE +
+                                   S5P_FIMV_ENC_COLFLG_SIZE +
+                                   S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                ctx->bank2_size = (enc_ref_y_size * 2) +
+                                   (enc_ref_c_size * 4);
+                break;
+        case S5P_MFC_CODEC_H263_ENC:
+                ctx->bank1_size = (enc_ref_y_size * 2) +
+                                   S5P_FIMV_ENC_UPMV_SIZE +
+                                   S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                ctx->bank2_size = (enc_ref_y_size * 2) +
+                                   (enc_ref_c_size * 4);
+                break;
+        default:
+                break;
+        }
+        /* Allocate only if memory from bank 1 is necessary */
+        if (ctx->bank1_size > 0) {
+                ctx->bank1_buf = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_size);
+                if (IS_ERR(ctx->bank1_buf)) {
+                        ctx->bank1_buf = NULL;
+                        printk(KERN_ERR
+                               "Buf alloc for decoding failed (port A)\n");
+                        return -ENOMEM;
+                }
+                ctx->bank1_phys = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_buf);
+                BUG_ON(ctx->bank1_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+        }
+        /* Allocate only if memory from bank 2 is necessary */
+        if (ctx->bank2_size > 0) {
+                ctx->bank2_buf = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_size);
+                if (IS_ERR(ctx->bank2_buf)) {
+                        ctx->bank2_buf = NULL;
+                        mfc_err("Buf alloc for decoding failed (port B)\n");
+                        return -ENOMEM;
+                }
+                ctx->bank2_phys = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_buf);
+                BUG_ON(ctx->bank2_phys & ((1 << MFC_BANK2_ALIGN_ORDER) - 1));
+        }
+        return 0;
+}
+
+/* Release buffers allocated for codec */
+void s5p_mfc_release_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->bank1_buf) {
+                vb2_dma_contig_memops.put(ctx->bank1_buf);
+                ctx->bank1_buf = NULL;
+                ctx->bank1_phys = 0;
+                ctx->bank1_size = 0;
+        }
+        if (ctx->bank2_buf) {
+                vb2_dma_contig_memops.put(ctx->bank2_buf);
+                ctx->bank2_buf = NULL;
+                ctx->bank2_phys = 0;
+                ctx->bank2_size = 0;
+        }
+}
+
+/* Allocate memory for instance data buffer */
+int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
+
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+                ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+                ctx->ctx.size = buf_size->h264_ctx;
+        else
+                ctx->ctx.size = buf_size->non_h264_ctx;
+        ctx->ctx.alloc = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx.size);
+        if (IS_ERR(ctx->ctx.alloc)) {
+                mfc_err("Allocating context buffer failed\n");
+                ctx->ctx.alloc = NULL;
+                return -ENOMEM;
+        }
+        ctx->ctx.dma = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx.alloc);
+        BUG_ON(ctx->ctx.dma & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+        ctx->ctx.ofs = OFFSETA(ctx->ctx.dma);
+        ctx->ctx.virt = vb2_dma_contig_memops.vaddr(ctx->ctx.alloc);
+        if (!ctx->ctx.virt) {
+                mfc_err("Remapping instance buffer failed\n");
+                vb2_dma_contig_memops.put(ctx->ctx.alloc);
+                ctx->ctx.alloc = NULL;
+                ctx->ctx.ofs = 0;
+                ctx->ctx.dma = 0;
+                return -ENOMEM;
+        }
+        /* Zero content of the allocated memory */
+        memset(ctx->ctx.virt, 0, ctx->ctx.size);
+        wmb();
+
+        /* Initialize shared memory */
+        ctx->shm.alloc = vb2_dma_contig_memops.alloc(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], buf_size->shm);
+        if (IS_ERR(ctx->shm.alloc)) {
+                mfc_err("failed to allocate shared memory\n");
+                return PTR_ERR(ctx->shm.alloc);
+        }
+        /* shared memory offset only keeps the offset from base (port a) */
+        ctx->shm.ofs = s5p_mfc_mem_cookie(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->shm.alloc)
+                                                                - dev->bank1;
+        BUG_ON(ctx->shm.ofs & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+
+        ctx->shm.virt = vb2_dma_contig_memops.vaddr(ctx->shm.alloc);
+        if (!ctx->shm.virt) {
+                vb2_dma_contig_memops.put(ctx->shm.alloc);
+                ctx->shm.alloc = NULL;
+                ctx->shm.ofs = 0;
+                mfc_err("failed to virt addr of shared memory\n");
+                return -ENOMEM;
+        }
+        memset((void *)ctx->shm.virt, 0, buf_size->shm);
+        wmb();
+        return 0;
+}
+
+/* Release instance buffer */
+void s5p_mfc_release_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->ctx.alloc) {
+                vb2_dma_contig_memops.put(ctx->ctx.alloc);
+                ctx->ctx.alloc = NULL;
+                ctx->ctx.ofs = 0;
+                ctx->ctx.virt = NULL;
+                ctx->ctx.dma = 0;
+        }
+        if (ctx->shm.alloc) {
+                vb2_dma_contig_memops.put(ctx->shm.alloc);
+                ctx->shm.alloc = NULL;
+                ctx->shm.ofs = 0;
+                ctx->shm.virt = NULL;
+        }
+}
+
+int s5p_mfc_alloc_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
+{
+        /* NOP */
+
+        return 0;
+}
+
+void s5p_mfc_release_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
+{
+        /* NOP */
+}
+
+static void s5p_mfc_write_info_v5(struct s5p_mfc_ctx *ctx, unsigned int data,
+                        unsigned int ofs)
+{
+        writel(data, (ctx->shm.virt + ofs));
+        wmb();
+}
+
+static unsigned int s5p_mfc_read_info_v5(struct s5p_mfc_ctx *ctx,
+                                unsigned int ofs)
+{
+        rmb();
+        return readl(ctx->shm.virt + ofs);
+}
+
+void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
+{
+        unsigned int guard_width, guard_height;
+
+        ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN);
+        ctx->buf_height = ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+        mfc_debug(2,
+                "SEQ Done: Movie dimensions %dx%d, buffer dimensions: %dx%d\n",
+                ctx->img_width, ctx->img_height, ctx->buf_width,
+                ctx->buf_height);
+
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC) {
+                ctx->luma_size = ALIGN(ctx->buf_width * ctx->buf_height,
+                                S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->chroma_size = ALIGN(ctx->buf_width *
+                                ALIGN((ctx->img_height >> 1),
+                                        S5P_FIMV_NV12MT_VALIGN),
+                                S5P_FIMV_DEC_BUF_ALIGN);
+                ctx->mv_size = ALIGN(ctx->buf_width *
+                                ALIGN((ctx->buf_height >> 2),
+                                        S5P_FIMV_NV12MT_VALIGN),
+                                S5P_FIMV_DEC_BUF_ALIGN);
+        } else {
+                guard_width =
+                        ALIGN(ctx->img_width + 24, S5P_FIMV_NV12MT_HALIGN);
+                guard_height =
+                        ALIGN(ctx->img_height + 16, S5P_FIMV_NV12MT_VALIGN);
+                ctx->luma_size = ALIGN(guard_width * guard_height,
+                                S5P_FIMV_DEC_BUF_ALIGN);
+
+                guard_width =
+                        ALIGN(ctx->img_width + 16, S5P_FIMV_NV12MT_HALIGN);
+                guard_height =
+                        ALIGN((ctx->img_height >> 1) + 4,
+                                        S5P_FIMV_NV12MT_VALIGN);
+                ctx->chroma_size = ALIGN(guard_width * guard_height,
+                                S5P_FIMV_DEC_BUF_ALIGN);
+
+                ctx->mv_size = 0;
+        }
+}
+
+void s5p_mfc_enc_calc_src_size_v5(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
+                ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN);
+
+                ctx->luma_size = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN)
+                        * ALIGN(ctx->img_height, S5P_FIMV_NV12M_LVALIGN);
+                ctx->chroma_size = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN)
+                        * ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12M_CVALIGN);
+
+                ctx->luma_size = ALIGN(ctx->luma_size, S5P_FIMV_NV12M_SALIGN);
+                ctx->chroma_size =
+                        ALIGN(ctx->chroma_size, S5P_FIMV_NV12M_SALIGN);
+        } else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT) {
+                ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN);
+
+                ctx->luma_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                        * ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+                ctx->chroma_size =
+                        ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                        * ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12MT_VALIGN);
+
+                ctx->luma_size = ALIGN(ctx->luma_size, S5P_FIMV_NV12MT_SALIGN);
+                ctx->chroma_size =
+                        ALIGN(ctx->chroma_size, S5P_FIMV_NV12MT_SALIGN);
+        }
+}
+
+/* Set registers for decoding temporary buffers */
+static void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
+
+        mfc_write(dev, OFFSETA(ctx->dsc.dma), S5P_FIMV_SI_CH0_DESC_ADR);
+        mfc_write(dev, buf_size->dsc, S5P_FIMV_SI_CH0_DESC_SIZE);
+}
+
+/* Set registers for shared buffer */
+static void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        mfc_write(dev, ctx->shm.ofs, S5P_FIMV_SI_CH0_HOST_WR_ADR);
+}
+
+/* Set registers for decoding stream buffer */
+int s5p_mfc_set_dec_stream_buffer_v5(struct s5p_mfc_ctx *ctx, int buf_addr,
+                  unsigned int start_num_byte, unsigned int buf_size)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, OFFSETA(buf_addr), S5P_FIMV_SI_CH0_SB_ST_ADR);
+        mfc_write(dev, ctx->dec_src_buf_size, S5P_FIMV_SI_CH0_CPB_SIZE);
+        mfc_write(dev, buf_size, S5P_FIMV_SI_CH0_SB_FRM_SIZE);
+        s5p_mfc_write_info_v5(ctx, start_num_byte, START_BYTE_NUM);
+        return 0;
+}
+
+/* Set decoding frame buffer */
+int s5p_mfc_set_dec_frame_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+        unsigned int frame_size, i;
+        unsigned int frame_size_ch, frame_size_mv;
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int dpb;
+        size_t buf_addr1, buf_addr2;
+        int buf_size1, buf_size2;
+
+        buf_addr1 = ctx->bank1_phys;
+        buf_size1 = ctx->bank1_size;
+        buf_addr2 = ctx->bank2_phys;
+        buf_size2 = ctx->bank2_size;
+        dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
+                                                ~S5P_FIMV_DPB_COUNT_MASK;
+        mfc_write(dev, ctx->total_dpb_count | dpb,
+                                                S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+        s5p_mfc_set_shared_buffer(ctx);
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_DEC:
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                                S5P_FIMV_H264_VERT_NB_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_VERT_NB_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_VERT_NB_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_NB_IP_ADR);
+                buf_addr1 += S5P_FIMV_DEC_NB_IP_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_NB_IP_SIZE;
+                break;
+        case S5P_MFC_CODEC_MPEG4_DEC:
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_NB_DCAC_ADR);
+                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_NB_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SA_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SP_ADR);
+                buf_addr1 += S5P_FIMV_DEC_STX_PARSER_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_STX_PARSER_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_OT_LINE_ADR);
+                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                break;
+        case S5P_MFC_CODEC_H263_DEC:
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_OT_LINE_ADR);
+                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_NB_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_SA_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_NB_DCAC_ADR);
+                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+                break;
+        case S5P_MFC_CODEC_VC1_DEC:
+        case S5P_MFC_CODEC_VC1RCV_DEC:
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_NB_DCAC_ADR);
+                buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_OT_LINE_ADR);
+                buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_UP_NB_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_SA_MV_ADR);
+                buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE3_ADR);
+                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE2_ADR);
+                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE1_ADR);
+                buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+                break;
+        case S5P_MFC_CODEC_MPEG2_DEC:
+                break;
+        default:
+                mfc_err("Unknown codec for decoding (%x)\n",
+                        ctx->codec_mode);
+                return -EINVAL;
+                break;
+        }
+        frame_size = ctx->luma_size;
+        frame_size_ch = ctx->chroma_size;
+        frame_size_mv = ctx->mv_size;
+        mfc_debug(2, "Frm size: %d ch: %d mv: %d\n", frame_size, frame_size_ch,
+                                                                frame_size_mv);
+        for (i = 0; i < ctx->total_dpb_count; i++) {
+                /* Bank2 */
+                mfc_debug(2, "Luma %d: %x\n", i,
+                                        ctx->dst_bufs[i].cookie.raw.luma);
+                mfc_write(dev, OFFSETB(ctx->dst_bufs[i].cookie.raw.luma),
+                                                S5P_FIMV_DEC_LUMA_ADR + i * 4);
+                mfc_debug(2, "\tChroma %d: %x\n", i,
+                                        ctx->dst_bufs[i].cookie.raw.chroma);
+                mfc_write(dev, OFFSETA(ctx->dst_bufs[i].cookie.raw.chroma),
+                                               S5P_FIMV_DEC_CHROMA_ADR + i * 4);
+                if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC) {
+                        mfc_debug(2, "\tBuf2: %x, size: %d\n",
+                                                        buf_addr2, buf_size2);
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                                S5P_FIMV_H264_MV_ADR + i * 4);
+                        buf_addr2 += frame_size_mv;
+                        buf_size2 -= frame_size_mv;
+                }
+        }
+        mfc_debug(2, "Buf1: %u, buf_size1: %d\n", buf_addr1, buf_size1);
+        mfc_debug(2, "Buf 1/2 size after: %d/%d (frames %d)\n",
+                        buf_size1,  buf_size2, ctx->total_dpb_count);
+        if (buf_size1 < 0 || buf_size2 < 0) {
+                mfc_debug(2, "Not enough memory has been allocated\n");
+                return -ENOMEM;
+        }
+        s5p_mfc_write_info_v5(ctx, frame_size, ALLOC_LUMA_DPB_SIZE);
+        s5p_mfc_write_info_v5(ctx, frame_size_ch, ALLOC_CHROMA_DPB_SIZE);
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC)
+                s5p_mfc_write_info_v5(ctx, frame_size_mv, ALLOC_MV_SIZE);
+        mfc_write(dev, ((S5P_FIMV_CH_INIT_BUFS & S5P_FIMV_CH_MASK)
+                                        << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
+                                                S5P_FIMV_SI_CH0_INST_ID);
+        return 0;
+}
+
+/* Set registers for encoding stream buffer */
+int s5p_mfc_set_enc_stream_buffer_v5(struct s5p_mfc_ctx *ctx,
+                unsigned long addr, unsigned int size)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, OFFSETA(addr), S5P_FIMV_ENC_SI_CH0_SB_ADR);
+        mfc_write(dev, size, S5P_FIMV_ENC_SI_CH0_SB_SIZE);
+        return 0;
+}
+
+void s5p_mfc_set_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
+                unsigned long y_addr, unsigned long c_addr)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, OFFSETB(y_addr), S5P_FIMV_ENC_SI_CH0_CUR_Y_ADR);
+        mfc_write(dev, OFFSETB(c_addr), S5P_FIMV_ENC_SI_CH0_CUR_C_ADR);
+}
+
+void s5p_mfc_get_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
+                unsigned long *y_addr, unsigned long *c_addr)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        *y_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_Y_ADDR)
+                                                        << MFC_OFFSET_SHIFT);
+        *c_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_C_ADDR)
+                                                        << MFC_OFFSET_SHIFT);
+}
+
+/* Set encoding ref & codec buffer */
+int s5p_mfc_set_enc_ref_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        size_t buf_addr1, buf_addr2;
+        size_t buf_size1, buf_size2;
+        unsigned int enc_ref_y_size, enc_ref_c_size;
+        unsigned int guard_width, guard_height;
+        int i;
+
+        buf_addr1 = ctx->bank1_phys;
+        buf_size1 = ctx->bank1_size;
+        buf_addr2 = ctx->bank2_phys;
+        buf_size2 = ctx->bank2_size;
+        enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                * ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+        enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC) {
+                enc_ref_c_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+                        * ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12MT_VALIGN);
+                enc_ref_c_size = ALIGN(enc_ref_c_size, S5P_FIMV_NV12MT_SALIGN);
+        } else {
+                guard_width = ALIGN(ctx->img_width + 16,
+                                                S5P_FIMV_NV12MT_HALIGN);
+                guard_height = ALIGN((ctx->img_height >> 1) + 4,
+                                                S5P_FIMV_NV12MT_VALIGN);
+                enc_ref_c_size = ALIGN(guard_width * guard_height,
+                                       S5P_FIMV_NV12MT_SALIGN);
+        }
+        mfc_debug(2, "buf_size1: %d, buf_size2: %d\n", buf_size1, buf_size2);
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_ENC:
+                for (i = 0; i < 2; i++) {
+                        mfc_write(dev, OFFSETA(buf_addr1),
+                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+                        buf_addr1 += enc_ref_y_size;
+                        buf_size1 -= enc_ref_y_size;
+
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_y_size;
+                        buf_size2 -= enc_ref_y_size;
+                }
+                for (i = 0; i < 4; i++) {
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_c_size;
+                        buf_size2 -= enc_ref_c_size;
+                }
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_UP_MV_ADR);
+                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                        S5P_FIMV_H264_COZERO_FLAG_ADR);
+                buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                        S5P_FIMV_H264_UP_INTRA_MD_ADR);
+                buf_addr1 += S5P_FIMV_ENC_INTRAMD_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_INTRAMD_SIZE;
+                mfc_write(dev, OFFSETB(buf_addr2),
+                                        S5P_FIMV_H264_UP_INTRA_PRED_ADR);
+                buf_addr2 += S5P_FIMV_ENC_INTRAPRED_SIZE;
+                buf_size2 -= S5P_FIMV_ENC_INTRAPRED_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                        S5P_FIMV_H264_NBOR_INFO_ADR);
+                buf_addr1 += S5P_FIMV_ENC_NBORINFO_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_NBORINFO_SIZE;
+                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+                        buf_size1, buf_size2);
+                break;
+        case S5P_MFC_CODEC_MPEG4_ENC:
+                for (i = 0; i < 2; i++) {
+                        mfc_write(dev, OFFSETA(buf_addr1),
+                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+                        buf_addr1 += enc_ref_y_size;
+                        buf_size1 -= enc_ref_y_size;
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_y_size;
+                        buf_size2 -= enc_ref_y_size;
+                }
+                for (i = 0; i < 4; i++) {
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_c_size;
+                        buf_size2 -= enc_ref_c_size;
+                }
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_MV_ADR);
+                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                                S5P_FIMV_MPEG4_COZERO_FLAG_ADR);
+                buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1),
+                                                S5P_FIMV_MPEG4_ACDC_COEF_ADR);
+                buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+                        buf_size1, buf_size2);
+                break;
+        case S5P_MFC_CODEC_H263_ENC:
+                for (i = 0; i < 2; i++) {
+                        mfc_write(dev, OFFSETA(buf_addr1),
+                                S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+                        buf_addr1 += enc_ref_y_size;
+                        buf_size1 -= enc_ref_y_size;
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_y_size;
+                        buf_size2 -= enc_ref_y_size;
+                }
+                for (i = 0; i < 4; i++) {
+                        mfc_write(dev, OFFSETB(buf_addr2),
+                                S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+                        buf_addr2 += enc_ref_c_size;
+                        buf_size2 -= enc_ref_c_size;
+                }
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_MV_ADR);
+                buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+                mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_ACDC_COEF_ADR);
+                buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
+                mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+                        buf_size1, buf_size2);
+                break;
+        default:
+                mfc_err("Unknown codec set for encoding: %d\n",
+                        ctx->codec_mode);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        unsigned int reg;
+        unsigned int shm;
+
+        /* width */
+        mfc_write(dev, ctx->img_width, S5P_FIMV_ENC_HSIZE_PX);
+        /* height */
+        mfc_write(dev, ctx->img_height, S5P_FIMV_ENC_VSIZE_PX);
+        /* pictype : enable, IDR period */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        reg |= (1 << 18);
+        reg &= ~(0xFFFF);
+        reg |= p->gop_size;
+        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        mfc_write(dev, 0, S5P_FIMV_ENC_B_RECON_WRITE_ON);
+        /* multi-slice control */
+        /* multi-slice MB number or bit size */
+        mfc_write(dev, p->slice_mode, S5P_FIMV_ENC_MSLICE_CTRL);
+        if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+                mfc_write(dev, p->slice_mb, S5P_FIMV_ENC_MSLICE_MB);
+        } else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+                mfc_write(dev, p->slice_bit, S5P_FIMV_ENC_MSLICE_BIT);
+        } else {
+                mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_MB);
+                mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_BIT);
+        }
+        /* cyclic intra refresh */
+        mfc_write(dev, p->intra_refresh_mb, S5P_FIMV_ENC_CIR_CTRL);
+        /* memory structure cur. frame */
+        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
+                mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
+        else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
+                mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
+        /* padding control & value */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PADDING_CTRL);
+        if (p->pad) {
+                /** enable */
+                reg |= (1 << 31);
+                /** cr value */
+                reg &= ~(0xFF << 16);
+                reg |= (p->pad_cr << 16);
+                /** cb value */
+                reg &= ~(0xFF << 8);
+                reg |= (p->pad_cb << 8);
+                /** y value */
+                reg &= ~(0xFF);
+                reg |= (p->pad_luma);
+        } else {
+                /** disable & all value clear */
+                reg = 0;
+        }
+        mfc_write(dev, reg, S5P_FIMV_ENC_PADDING_CTRL);
+        /* rate control config. */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+        /** frame-level rate control */
+        reg &= ~(0x1 << 9);
+        reg |= (p->rc_frame << 9);
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+        /* bit rate */
+        if (p->rc_frame)
+                mfc_write(dev, p->rc_bitrate,
+                        S5P_FIMV_ENC_RC_BIT_RATE);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_RC_BIT_RATE);
+        /* reaction coefficient */
+        if (p->rc_frame)
+                mfc_write(dev, p->rc_reaction_coeff, S5P_FIMV_ENC_RC_RPARA);
+        shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+        /* seq header ctrl */
+        shm &= ~(0x1 << 3);
+        shm |= (p->seq_hdr_mode << 3);
+        /* frame skip mode */
+        shm &= ~(0x3 << 1);
+        shm |= (p->frame_skip_mode << 1);
+        s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+        /* fixed target bit */
+        s5p_mfc_write_info_v5(ctx, p->fixed_target_bit, RC_CONTROL_CONFIG);
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_h264_enc_params *p_264 = &p->codec.h264;
+        unsigned int reg;
+        unsigned int shm;
+
+        s5p_mfc_set_enc_params(ctx);
+        /* pictype : number of B */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        /* num_b_frame - 0 ~ 2 */
+        reg &= ~(0x3 << 16);
+        reg |= (p->num_b_frame << 16);
+        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        /* profile & level */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
+        /* level */
+        reg &= ~(0xFF << 8);
+        reg |= (p_264->level << 8);
+        /* profile - 0 ~ 2 */
+        reg &= ~(0x3F);
+        reg |= p_264->profile;
+        mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
+        /* interlace  */
+        mfc_write(dev, p_264->interlace, S5P_FIMV_ENC_PIC_STRUCT);
+        /* height */
+        if (p_264->interlace)
+                mfc_write(dev, ctx->img_height >> 1, S5P_FIMV_ENC_VSIZE_PX);
+        /* loopfilter ctrl */
+        mfc_write(dev, p_264->loop_filter_mode, S5P_FIMV_ENC_LF_CTRL);
+        /* loopfilter alpha offset */
+        if (p_264->loop_filter_alpha < 0) {
+                reg = 0x10;
+                reg |= (0xFF - p_264->loop_filter_alpha) + 1;
+        } else {
+                reg = 0x00;
+                reg |= (p_264->loop_filter_alpha & 0xF);
+        }
+        mfc_write(dev, reg, S5P_FIMV_ENC_ALPHA_OFF);
+        /* loopfilter beta offset */
+        if (p_264->loop_filter_beta < 0) {
+                reg = 0x10;
+                reg |= (0xFF - p_264->loop_filter_beta) + 1;
+        } else {
+                reg = 0x00;
+                reg |= (p_264->loop_filter_beta & 0xF);
+        }
+        mfc_write(dev, reg, S5P_FIMV_ENC_BETA_OFF);
+        /* entropy coding mode */
+        if (p_264->entropy_mode == V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC)
+                mfc_write(dev, 1, S5P_FIMV_ENC_H264_ENTROPY_MODE);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_H264_ENTROPY_MODE);
+        /* number of ref. picture */
+        reg = mfc_read(dev, S5P_FIMV_ENC_H264_NUM_OF_REF);
+        /* num of ref. pictures of P */
+        reg &= ~(0x3 << 5);
+        reg |= (p_264->num_ref_pic_4p << 5);
+        /* max number of ref. pictures */
+        reg &= ~(0x1F);
+        reg |= p_264->max_ref_pic;
+        mfc_write(dev, reg, S5P_FIMV_ENC_H264_NUM_OF_REF);
+        /* 8x8 transform enable */
+        mfc_write(dev, p_264->_8x8_transform, S5P_FIMV_ENC_H264_TRANS_FLAG);
+        /* rate control config. */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+        /* macroblock level rate control */
+        reg &= ~(0x1 << 8);
+        reg |= (p->rc_mb << 8);
+        /* frame QP */
+        reg &= ~(0x3F);
+        reg |= p_264->rc_frame_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+        /* frame rate */
+        if (p->rc_frame && p->rc_framerate_denom)
+                mfc_write(dev, p->rc_framerate_num * 1000
+                        / p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+        /* max & min value of QP */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+        /* max QP */
+        reg &= ~(0x3F << 8);
+        reg |= (p_264->rc_max_qp << 8);
+        /* min QP */
+        reg &= ~(0x3F);
+        reg |= p_264->rc_min_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+        /* macroblock adaptive scaling features */
+        if (p->rc_mb) {
+                reg = mfc_read(dev, S5P_FIMV_ENC_RC_MB_CTRL);
+                /* dark region */
+                reg &= ~(0x1 << 3);
+                reg |= (p_264->rc_mb_dark << 3);
+                /* smooth region */
+                reg &= ~(0x1 << 2);
+                reg |= (p_264->rc_mb_smooth << 2);
+                /* static region */
+                reg &= ~(0x1 << 1);
+                reg |= (p_264->rc_mb_static << 1);
+                /* high activity region */
+                reg &= ~(0x1);
+                reg |= p_264->rc_mb_activity;
+                mfc_write(dev, reg, S5P_FIMV_ENC_RC_MB_CTRL);
+        }
+        if (!p->rc_frame && !p->rc_mb) {
+                shm = s5p_mfc_read_info_v5(ctx, P_B_FRAME_QP);
+                shm &= ~(0xFFF);
+                shm |= ((p_264->rc_b_frame_qp & 0x3F) << 6);
+                shm |= (p_264->rc_p_frame_qp & 0x3F);
+                s5p_mfc_write_info_v5(ctx, shm, P_B_FRAME_QP);
+        }
+        /* extended encoder ctrl */
+        shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+        /* AR VUI control */
+        shm &= ~(0x1 << 15);
+        shm |= (p_264->vui_sar << 1);
+        s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+        if (p_264->vui_sar) {
+                /* aspect ration IDC */
+                shm = s5p_mfc_read_info_v5(ctx, SAMPLE_ASPECT_RATIO_IDC);
+                shm &= ~(0xFF);
+                shm |= p_264->vui_sar_idc;
+                s5p_mfc_write_info_v5(ctx, shm, SAMPLE_ASPECT_RATIO_IDC);
+                if (p_264->vui_sar_idc == 0xFF) {
+                        /* sample  AR info */
+                        shm = s5p_mfc_read_info_v5(ctx, EXTENDED_SAR);
+                        shm &= ~(0xFFFFFFFF);
+                        shm |= p_264->vui_ext_sar_width << 16;
+                        shm |= p_264->vui_ext_sar_height;
+                        s5p_mfc_write_info_v5(ctx, shm, EXTENDED_SAR);
+                }
+        }
+        /* intra picture period for H.264 */
+        shm = s5p_mfc_read_info_v5(ctx, H264_I_PERIOD);
+        /* control */
+        shm &= ~(0x1 << 16);
+        shm |= (p_264->open_gop << 16);
+        /* value */
+        if (p_264->open_gop) {
+                shm &= ~(0xFFFF);
+                shm |= p_264->open_gop_size;
+        }
+        s5p_mfc_write_info_v5(ctx, shm, H264_I_PERIOD);
+        /* extended encoder ctrl */
+        shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                shm &= ~(0xFFFF << 16);
+                shm |= (p_264->cpb_size << 16);
+        }
+        s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
+        unsigned int reg;
+        unsigned int shm;
+        unsigned int framerate;
+
+        s5p_mfc_set_enc_params(ctx);
+        /* pictype : number of B */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        /* num_b_frame - 0 ~ 2 */
+        reg &= ~(0x3 << 16);
+        reg |= (p->num_b_frame << 16);
+        mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+        /* profile & level */
+        reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
+        /* level */
+        reg &= ~(0xFF << 8);
+        reg |= (p_mpeg4->level << 8);
+        /* profile - 0 ~ 2 */
+        reg &= ~(0x3F);
+        reg |= p_mpeg4->profile;
+        mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
+        /* quarter_pixel */
+        mfc_write(dev, p_mpeg4->quarter_pixel, S5P_FIMV_ENC_MPEG4_QUART_PXL);
+        /* qp */
+        if (!p->rc_frame) {
+                shm = s5p_mfc_read_info_v5(ctx, P_B_FRAME_QP);
+                shm &= ~(0xFFF);
+                shm |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 6);
+                shm |= (p_mpeg4->rc_p_frame_qp & 0x3F);
+                s5p_mfc_write_info_v5(ctx, shm, P_B_FRAME_QP);
+        }
+        /* frame rate */
+        if (p->rc_frame) {
+                if (p->rc_framerate_denom > 0) {
+                        framerate = p->rc_framerate_num * 1000 /
+                                                p->rc_framerate_denom;
+                        mfc_write(dev, framerate,
+                                S5P_FIMV_ENC_RC_FRAME_RATE);
+                        shm = s5p_mfc_read_info_v5(ctx, RC_VOP_TIMING);
+                        shm &= ~(0xFFFFFFFF);
+                        shm |= (1 << 31);
+                        shm |= ((p->rc_framerate_num & 0x7FFF) << 16);
+                        shm |= (p->rc_framerate_denom & 0xFFFF);
+                        s5p_mfc_write_info_v5(ctx, shm, RC_VOP_TIMING);
+                }
+        } else {
+                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+        }
+        /* rate control config. */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+        /* frame QP */
+        reg &= ~(0x3F);
+        reg |= p_mpeg4->rc_frame_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+        /* max & min value of QP */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+        /* max QP */
+        reg &= ~(0x3F << 8);
+        reg |= (p_mpeg4->rc_max_qp << 8);
+        /* min QP */
+        reg &= ~(0x3F);
+        reg |= p_mpeg4->rc_min_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+        /* extended encoder ctrl */
+        shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                shm &= ~(0xFFFF << 16);
+                shm |= (p->vbv_size << 16);
+        }
+        s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
+        unsigned int reg;
+        unsigned int shm;
+
+        s5p_mfc_set_enc_params(ctx);
+        /* qp */
+        if (!p->rc_frame) {
+                shm = s5p_mfc_read_info_v5(ctx, P_B_FRAME_QP);
+                shm &= ~(0xFFF);
+                shm |= (p_h263->rc_p_frame_qp & 0x3F);
+                s5p_mfc_write_info_v5(ctx, shm, P_B_FRAME_QP);
+        }
+        /* frame rate */
+        if (p->rc_frame && p->rc_framerate_denom)
+                mfc_write(dev, p->rc_framerate_num * 1000
+                        / p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+        /* rate control config. */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+        /* frame QP */
+        reg &= ~(0x3F);
+        reg |= p_h263->rc_frame_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+        /* max & min value of QP */
+        reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+        /* max QP */
+        reg &= ~(0x3F << 8);
+        reg |= (p_h263->rc_max_qp << 8);
+        /* min QP */
+        reg &= ~(0x3F);
+        reg |= p_h263->rc_min_qp;
+        mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+        /* extended encoder ctrl */
+        shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                shm &= ~(0xFFFF << 16);
+                shm |= (p->vbv_size << 16);
+        }
+        s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+        return 0;
+}
+
+/* Initialize decoding */
+int s5p_mfc_init_decode_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        s5p_mfc_set_shared_buffer(ctx);
+        /* Setup loop filter, for decoding this is only valid for MPEG4 */
+        if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_DEC)
+                mfc_write(dev, ctx->loop_filter_mpeg4, S5P_FIMV_ENC_LF_CTRL);
+        else
+                mfc_write(dev, 0, S5P_FIMV_ENC_LF_CTRL);
+        mfc_write(dev, ((ctx->slice_interface & S5P_FIMV_SLICE_INT_MASK) <<
+                S5P_FIMV_SLICE_INT_SHIFT) | (ctx->display_delay_enable <<
+                S5P_FIMV_DDELAY_ENA_SHIFT) | ((ctx->display_delay &
+                S5P_FIMV_DDELAY_VAL_MASK) << S5P_FIMV_DDELAY_VAL_SHIFT),
+                S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+        mfc_write(dev,
+        ((S5P_FIMV_CH_SEQ_HEADER & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
+                                | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+        return 0;
+}
+
+static void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int dpb;
+
+        if (flush)
+                dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) | (
+                        S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
+        else
+                dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
+                        ~(S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
+        mfc_write(dev, dpb, S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+}
+
+/* Decode a single frame */
+int s5p_mfc_decode_one_frame_v5(struct s5p_mfc_ctx *ctx,
+                                        enum s5p_mfc_decode_arg last_frame)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_write(dev, ctx->dec_dst_flag, S5P_FIMV_SI_CH0_RELEASE_BUF);
+        s5p_mfc_set_shared_buffer(ctx);
+        s5p_mfc_set_flush(ctx, ctx->dpb_flush_flag);
+        /* Issue different commands to instance basing on whether it
+         * is the last frame or not. */
+        switch (last_frame) {
+        case MFC_DEC_FRAME:
+                mfc_write(dev, ((S5P_FIMV_CH_FRAME_START & S5P_FIMV_CH_MASK) <<
+                S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+                break;
+        case MFC_DEC_LAST_FRAME:
+                mfc_write(dev, ((S5P_FIMV_CH_LAST_FRAME & S5P_FIMV_CH_MASK) <<
+                S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+                break;
+        case MFC_DEC_RES_CHANGE:
+                mfc_write(dev, ((S5P_FIMV_CH_FRAME_START_REALLOC &
+                S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
+                S5P_FIMV_SI_CH0_INST_ID);
+                break;
+        }
+        mfc_debug(2, "Decoding a usual frame\n");
+        return 0;
+}
+
+int s5p_mfc_init_encode_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+                s5p_mfc_set_enc_params_h264(ctx);
+        else if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_ENC)
+                s5p_mfc_set_enc_params_mpeg4(ctx);
+        else if (ctx->codec_mode == S5P_MFC_CODEC_H263_ENC)
+                s5p_mfc_set_enc_params_h263(ctx);
+        else {
+                mfc_err("Unknown codec for encoding (%x)\n",
+                        ctx->codec_mode);
+                return -EINVAL;
+        }
+        s5p_mfc_set_shared_buffer(ctx);
+        mfc_write(dev, ((S5P_FIMV_CH_SEQ_HEADER << 16) & 0x70000) |
+                (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+        return 0;
+}
+
+/* Encode a single frame */
+int s5p_mfc_encode_one_frame_v5(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        int cmd;
+        /* memory structure cur. frame */
+        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
+                mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
+        else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
+                mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
+        s5p_mfc_set_shared_buffer(ctx);
+
+        if (ctx->state == MFCINST_FINISHING)
+                cmd = S5P_FIMV_CH_LAST_FRAME;
+        else
+                cmd = S5P_FIMV_CH_FRAME_START;
+        mfc_write(dev, ((cmd & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
+                                | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+
+        return 0;
+}
+
+static int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
+{
+        unsigned long flags;
+        int new_ctx;
+        int cnt;
+
+        spin_lock_irqsave(&dev->condlock, flags);
+        new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
+        cnt = 0;
+        while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
+                new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
+                if (++cnt > MFC_NUM_CONTEXTS) {
+                        /* No contexts to run */
+                        spin_unlock_irqrestore(&dev->condlock, flags);
+                        return -EAGAIN;
+                }
+        }
+        spin_unlock_irqrestore(&dev->condlock, flags);
+        return new_ctx;
+}
+
+static void s5p_mfc_run_res_change(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        s5p_mfc_set_dec_stream_buffer_v5(ctx, 0, 0, 0);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_decode_one_frame_v5(ctx, MFC_DEC_RES_CHANGE);
+}
+
+static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf *temp_vb;
+        unsigned long flags;
+        unsigned int index;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+        /* Frames are being decoded */
+        if (list_empty(&ctx->src_queue)) {
+                mfc_debug(2, "No src buffers\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+        /* Get the next source buffer */
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        temp_vb->flags |= MFC_BUF_FLAG_USED;
+        s5p_mfc_set_dec_stream_buffer_v5(ctx,
+                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
+                ctx->consumed_stream, temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        index = temp_vb->b->v4l2_buf.index;
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        if (temp_vb->b->v4l2_planes[0].bytesused == 0) {
+                last_frame = MFC_DEC_LAST_FRAME;
+                mfc_debug(2, "Setting ctx->state to FINISHING\n");
+                ctx->state = MFCINST_FINISHING;
+        }
+        s5p_mfc_decode_one_frame_v5(ctx, last_frame);
+        return 0;
+}
+
+static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *dst_mb;
+        struct s5p_mfc_buf *src_mb;
+        unsigned long src_y_addr, src_c_addr, dst_addr;
+        unsigned int dst_size;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+        if (list_empty(&ctx->src_queue) && ctx->state != MFCINST_FINISHING) {
+                mfc_debug(2, "no src buffers\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+        if (list_empty(&ctx->dst_queue)) {
+                mfc_debug(2, "no dst buffers\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+        if (list_empty(&ctx->src_queue)) {
+                /* send null frame */
+                s5p_mfc_set_enc_frame_buffer_v5(ctx, dev->bank2, dev->bank2);
+                src_mb = NULL;
+        } else {
+                src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
+                                                                        list);
+                src_mb->flags |= MFC_BUF_FLAG_USED;
+                if (src_mb->b->v4l2_planes[0].bytesused == 0) {
+                        /* send null frame */
+                        s5p_mfc_set_enc_frame_buffer_v5(ctx, dev->bank2,
+                                                                dev->bank2);
+                        ctx->state = MFCINST_FINISHING;
+                } else {
+                        src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b,
+                                                                        0);
+                        src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b,
+                                                                        1);
+                        s5p_mfc_set_enc_frame_buffer_v5(ctx, src_y_addr,
+                                                                src_c_addr);
+                        if (src_mb->flags & MFC_BUF_FLAG_EOS)
+                                ctx->state = MFCINST_FINISHING;
+                }
+        }
+        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+        dst_mb->flags |= MFC_BUF_FLAG_USED;
+        dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
+        dst_size = vb2_plane_size(dst_mb->b, 0);
+        s5p_mfc_set_enc_stream_buffer_v5(ctx, dst_addr, dst_size);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        mfc_debug(2, "encoding buffer with index=%d state=%d",
+                        src_mb ? src_mb->b->v4l2_buf.index : -1, ctx->state);
+        s5p_mfc_encode_one_frame_v5(ctx);
+        return 0;
+}
+
+static void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *temp_vb;
+
+        /* Initializing decoding - parsing header */
+        spin_lock_irqsave(&dev->irqlock, flags);
+        mfc_debug(2, "Preparing to init decoding\n");
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        s5p_mfc_set_dec_desc_buffer(ctx);
+        mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
+        s5p_mfc_set_dec_stream_buffer_v5(ctx,
+                                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
+                                0, temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_init_decode_v5(ctx);
+}
+
+static void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *dst_mb;
+        unsigned long dst_addr;
+        unsigned int dst_size;
+
+        s5p_mfc_set_enc_ref_buffer_v5(ctx);
+        spin_lock_irqsave(&dev->irqlock, flags);
+        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+        dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
+        dst_size = vb2_plane_size(dst_mb->b, 0);
+        s5p_mfc_set_enc_stream_buffer_v5(ctx, dst_addr, dst_size);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_init_encode_v5(ctx);
+}
+
+static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *temp_vb;
+        int ret;
+
+        /*
+         * Header was parsed now starting processing
+         * First set the output frame buffers
+         */
+        if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
+                mfc_err("It seems that not all destionation buffers were "
+                        "mmaped\nMFC requires that all destination are mmaped "
+                        "before starting processing\n");
+                return -EAGAIN;
+        }
+        spin_lock_irqsave(&dev->irqlock, flags);
+        if (list_empty(&ctx->src_queue)) {
+                mfc_err("Header has been deallocated in the middle of"
+                        " initialization\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EIO;
+        }
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
+        s5p_mfc_set_dec_stream_buffer_v5(ctx,
+                                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
+                                0, temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        ret = s5p_mfc_set_dec_frame_buffer_v5(ctx);
+        if (ret) {
+                mfc_err("Failed to alloc frame mem\n");
+                ctx->state = MFCINST_ERROR;
+        }
+        return ret;
+}
+
+/* Try running an operation on hardware */
+void s5p_mfc_try_run_v5(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_ctx *ctx;
+        int new_ctx;
+        unsigned int ret = 0;
+
+        if (test_bit(0, &dev->enter_suspend)) {
+                mfc_debug(1, "Entering suspend so do not schedule any jobs\n");
+                return;
+        }
+        /* Check whether hardware is not running */
+        if (test_and_set_bit(0, &dev->hw_lock) != 0) {
+                /* This is perfectly ok, the scheduled ctx should wait */
+                mfc_debug(1, "Couldn't lock HW\n");
+                return;
+        }
+        /* Choose the context to run */
+        new_ctx = s5p_mfc_get_new_ctx(dev);
+        if (new_ctx < 0) {
+                /* No contexts to run */
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0) {
+                        mfc_err("Failed to unlock hardware\n");
+                        return;
+                }
+                mfc_debug(1, "No ctx is scheduled to be run\n");
+                return;
+        }
+        ctx = dev->ctx[new_ctx];
+        /* Got context to run in ctx */
+        /*
+         * Last frame has already been sent to MFC.
+         * Now obtaining frames from MFC buffer
+         */
+        s5p_mfc_clock_on();
+        if (ctx->type == MFCINST_DECODER) {
+                s5p_mfc_set_dec_desc_buffer(ctx);
+                switch (ctx->state) {
+                case MFCINST_FINISHING:
+                        s5p_mfc_run_dec_frame(ctx, MFC_DEC_LAST_FRAME);
+                        break;
+                case MFCINST_RUNNING:
+                        ret = s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
+                        break;
+                case MFCINST_INIT:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_RETURN_INST:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_GOT_INST:
+                        s5p_mfc_run_init_dec(ctx);
+                        break;
+                case MFCINST_HEAD_PARSED:
+                        ret = s5p_mfc_run_init_dec_buffers(ctx);
+                        mfc_debug(1, "head parsed\n");
+                        break;
+                case MFCINST_RES_CHANGE_INIT:
+                        s5p_mfc_run_res_change(ctx);
+                        break;
+                case MFCINST_RES_CHANGE_FLUSH:
+                        s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
+                        break;
+                case MFCINST_RES_CHANGE_END:
+                        mfc_debug(2, "Finished remaining frames after resolution change\n");
+                        ctx->capture_state = QUEUE_FREE;
+                        mfc_debug(2, "Will re-init the codec\n");
+                        s5p_mfc_run_init_dec(ctx);
+                        break;
+                default:
+                        ret = -EAGAIN;
+                }
+        } else if (ctx->type == MFCINST_ENCODER) {
+                switch (ctx->state) {
+                case MFCINST_FINISHING:
+                case MFCINST_RUNNING:
+                        ret = s5p_mfc_run_enc_frame(ctx);
+                        break;
+                case MFCINST_INIT:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_RETURN_INST:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_GOT_INST:
+                        s5p_mfc_run_init_enc(ctx);
+                        break;
+                default:
+                        ret = -EAGAIN;
+                }
+        } else {
+                mfc_err("Invalid context type: %d\n", ctx->type);
+                ret = -EAGAIN;
+        }
+
+        if (ret) {
+                /* Free hardware lock */
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                        mfc_err("Failed to unlock hardware\n");
+
+                /* This is in deed imporant, as no operation has been
+                 * scheduled, reduce the clock count as no one will
+                 * ever do this, because no interrupt related to this try_run
+                 * will ever come from hardware. */
+                s5p_mfc_clock_off();
+        }
+}
+
+
+void s5p_mfc_cleanup_queue_v5(struct list_head *lh, struct vb2_queue *vq)
+{
+        struct s5p_mfc_buf *b;
+        int i;
+
+        while (!list_empty(lh)) {
+                b = list_entry(lh->next, struct s5p_mfc_buf, list);
+                for (i = 0; i < b->b->num_planes; i++)
+                        vb2_set_plane_payload(b->b, i, 0);
+                vb2_buffer_done(b->b, VB2_BUF_STATE_ERROR);
+                list_del(&b->list);
+        }
+}
+
+void s5p_mfc_clear_int_flags_v5(struct s5p_mfc_dev *dev)
+{
+        mfc_write(dev, 0, S5P_FIMV_RISC_HOST_INT);
+        mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
+        mfc_write(dev, 0xffff, S5P_FIMV_SI_RTN_CHID);
+}
+
+int s5p_mfc_get_dspl_y_adr_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_DISPLAY_Y_ADR) << MFC_OFFSET_SHIFT;
+}
+
+int s5p_mfc_get_dec_y_adr_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_DECODE_Y_ADR) << MFC_OFFSET_SHIFT;
+}
+
+int s5p_mfc_get_dspl_status_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_DISPLAY_STATUS);
+}
+
+int s5p_mfc_get_dec_status_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_DECODE_STATUS);
+}
+
+int s5p_mfc_get_dec_frame_type_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_DECODE_FRAME_TYPE) &
+                S5P_FIMV_DECODE_FRAME_MASK;
+}
+
+int s5p_mfc_get_disp_frame_type_v5(struct s5p_mfc_ctx *ctx)
+{
+        return (s5p_mfc_read_info_v5(ctx, DISP_PIC_FRAME_TYPE) >>
+                        S5P_FIMV_SHARED_DISP_FRAME_TYPE_SHIFT) &
+                        S5P_FIMV_DECODE_FRAME_MASK;
+}
+
+int s5p_mfc_get_consumed_stream_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_CONSUMED_BYTES);
+}
+
+int s5p_mfc_get_int_reason_v5(struct s5p_mfc_dev *dev)
+{
+        int reason;
+        reason = mfc_read(dev, S5P_FIMV_RISC2HOST_CMD) &
+                S5P_FIMV_RISC2HOST_CMD_MASK;
+        switch (reason) {
+        case S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET:
+                reason = S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET:
+                reason = S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_SEQ_DONE_RET:
+                reason = S5P_MFC_R2H_CMD_SEQ_DONE_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_FRAME_DONE_RET:
+                reason = S5P_MFC_R2H_CMD_FRAME_DONE_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_SLICE_DONE_RET:
+                reason = S5P_MFC_R2H_CMD_SLICE_DONE_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_SYS_INIT_RET:
+                reason = S5P_MFC_R2H_CMD_SYS_INIT_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_FW_STATUS_RET:
+                reason = S5P_MFC_R2H_CMD_FW_STATUS_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_SLEEP_RET:
+                reason = S5P_MFC_R2H_CMD_SLEEP_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_WAKEUP_RET:
+                reason = S5P_MFC_R2H_CMD_WAKEUP_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET:
+                reason = S5P_MFC_R2H_CMD_INIT_BUFFERS_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_ENC_COMPLETE_RET:
+                reason = S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET;
+                break;
+        case S5P_FIMV_R2H_CMD_ERR_RET:
+                reason = S5P_MFC_R2H_CMD_ERR_RET;
+                break;
+        default:
+                reason = S5P_MFC_R2H_CMD_EMPTY;
+        };
+        return reason;
+}
+
+int s5p_mfc_get_int_err_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_RISC2HOST_ARG2);
+}
+
+int s5p_mfc_err_dec_v5(unsigned int err)
+{
+        return (err & S5P_FIMV_ERR_DEC_MASK) >> S5P_FIMV_ERR_DEC_SHIFT;
+}
+
+int s5p_mfc_err_dspl_v5(unsigned int err)
+{
+        return (err & S5P_FIMV_ERR_DSPL_MASK) >> S5P_FIMV_ERR_DSPL_SHIFT;
+}
+
+int s5p_mfc_get_img_width_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_HRESOL);
+}
+
+int s5p_mfc_get_img_height_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_VRESOL);
+}
+
+int s5p_mfc_get_dpb_count_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_SI_BUF_NUMBER);
+}
+
+int s5p_mfc_get_mv_count_v5(struct s5p_mfc_dev *dev)
+{
+        /* NOP */
+        return -1;
+}
+
+int s5p_mfc_get_inst_no_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_RISC2HOST_ARG1);
+}
+
+int s5p_mfc_get_enc_strm_size_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_ENC_SI_STRM_SIZE);
+}
+
+int s5p_mfc_get_enc_slice_type_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_ENC_SI_SLICE_TYPE);
+}
+
+int s5p_mfc_get_enc_dpb_count_v5(struct s5p_mfc_dev *dev)
+{
+        return -1;
+}
+
+int s5p_mfc_get_enc_pic_count_v5(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT);
+}
+
+int s5p_mfc_get_sei_avail_status_v5(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v5(ctx, FRAME_PACK_SEI_AVAIL);
+}
+
+int s5p_mfc_get_mvc_num_views_v5(struct s5p_mfc_dev *dev)
+{
+        return -1;
+}
+
+int s5p_mfc_get_mvc_view_id_v5(struct s5p_mfc_dev *dev)
+{
+        return -1;
+}
+
+unsigned int s5p_mfc_get_pic_type_top_v5(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v5(ctx, PIC_TIME_TOP);
+}
+
+unsigned int s5p_mfc_get_pic_type_bot_v5(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v5(ctx, PIC_TIME_BOT);
+}
+
+unsigned int s5p_mfc_get_crop_info_h_v5(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v5(ctx, CROP_INFO_H);
+}
+
+unsigned int s5p_mfc_get_crop_info_v_v5(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v5(ctx, CROP_INFO_V);
+}
+
+/* Initialize opr function pointers for MFC v5 */
+static struct s5p_mfc_hw_ops s5p_mfc_ops_v5 = {
+        .alloc_dec_temp_buffers = s5p_mfc_alloc_dec_temp_buffers_v5,
+        .release_dec_desc_buffer = s5p_mfc_release_dec_desc_buffer_v5,
+        .alloc_codec_buffers = s5p_mfc_alloc_codec_buffers_v5,
+        .release_codec_buffers = s5p_mfc_release_codec_buffers_v5,
+        .alloc_instance_buffer = s5p_mfc_alloc_instance_buffer_v5,
+        .release_instance_buffer = s5p_mfc_release_instance_buffer_v5,
+        .alloc_dev_context_buffer = s5p_mfc_alloc_dev_context_buffer_v5,
+        .release_dev_context_buffer = s5p_mfc_release_dev_context_buffer_v5,
+        .dec_calc_dpb_size = s5p_mfc_dec_calc_dpb_size_v5,
+        .enc_calc_src_size = s5p_mfc_enc_calc_src_size_v5,
+        .set_dec_stream_buffer = s5p_mfc_set_dec_stream_buffer_v5,
+        .set_dec_frame_buffer = s5p_mfc_set_dec_frame_buffer_v5,
+        .set_enc_stream_buffer = s5p_mfc_set_enc_stream_buffer_v5,
+        .set_enc_frame_buffer = s5p_mfc_set_enc_frame_buffer_v5,
+        .get_enc_frame_buffer = s5p_mfc_get_enc_frame_buffer_v5,
+        .set_enc_ref_buffer = s5p_mfc_set_enc_ref_buffer_v5,
+        .init_decode = s5p_mfc_init_decode_v5,
+        .init_encode = s5p_mfc_init_encode_v5,
+        .encode_one_frame = s5p_mfc_encode_one_frame_v5,
+        .try_run = s5p_mfc_try_run_v5,
+        .cleanup_queue = s5p_mfc_cleanup_queue_v5,
+        .clear_int_flags = s5p_mfc_clear_int_flags_v5,
+        .write_info = s5p_mfc_write_info_v5,
+        .read_info = s5p_mfc_read_info_v5,
+        .get_dspl_y_adr = s5p_mfc_get_dspl_y_adr_v5,
+        .get_dec_y_adr = s5p_mfc_get_dec_y_adr_v5,
+        .get_dspl_status = s5p_mfc_get_dspl_status_v5,
+        .get_dec_status = s5p_mfc_get_dec_status_v5,
+        .get_dec_frame_type = s5p_mfc_get_dec_frame_type_v5,
+        .get_disp_frame_type = s5p_mfc_get_disp_frame_type_v5,
+        .get_consumed_stream = s5p_mfc_get_consumed_stream_v5,
+        .get_int_reason = s5p_mfc_get_int_reason_v5,
+        .get_int_err = s5p_mfc_get_int_err_v5,
+        .err_dec = s5p_mfc_err_dec_v5,
+        .err_dspl = s5p_mfc_err_dspl_v5,
+        .get_img_width = s5p_mfc_get_img_width_v5,
+        .get_img_height = s5p_mfc_get_img_height_v5,
+        .get_dpb_count = s5p_mfc_get_dpb_count_v5,
+        .get_mv_count = s5p_mfc_get_mv_count_v5,
+        .get_inst_no = s5p_mfc_get_inst_no_v5,
+        .get_enc_strm_size = s5p_mfc_get_enc_strm_size_v5,
+        .get_enc_slice_type = s5p_mfc_get_enc_slice_type_v5,
+        .get_enc_dpb_count = s5p_mfc_get_enc_dpb_count_v5,
+        .get_enc_pic_count = s5p_mfc_get_enc_pic_count_v5,
+        .get_sei_avail_status = s5p_mfc_get_sei_avail_status_v5,
+        .get_mvc_num_views = s5p_mfc_get_mvc_num_views_v5,
+        .get_mvc_view_id = s5p_mfc_get_mvc_view_id_v5,
+        .get_pic_type_top = s5p_mfc_get_pic_type_top_v5,
+        .get_pic_type_bot = s5p_mfc_get_pic_type_bot_v5,
+        .get_crop_info_h = s5p_mfc_get_crop_info_h_v5,
+        .get_crop_info_v = s5p_mfc_get_crop_info_v_v5,
+};
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v5(void)
+{
+        return &s5p_mfc_ops_v5;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_shm.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.h
index 416ebd7ba35a..ffee39a127d5 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_shm.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.h
@@ -1,17 +1,22 @@
 /*
- * linux/drivers/media/platform/s5p-mfc/s5p_mfc_shm.h
+ * drivers/media/platform/samsung/mfc5/s5p_mfc_opr_v5.h
  *
- * Copyright (c) 2011 Samsung Electronics Co., Ltd.
- *              http://www.samsung.com/
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * Contains declarations of hw related functions.
+ *
+ * Kamil Debski, Copyright (C) 2011 Samsung Electronics
+ * http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
  */
 
-#ifndef S5P_MFC_SHM_H_
-#define S5P_MFC_SHM_H_
+#ifndef S5P_MFC_OPR_V5_H_
+#define S5P_MFC_OPR_V5_H_
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_opr.h"
 
 enum MFC_SHM_OFS {
         EXTENEDED_DECODE_STATUS = 0x00, /* D */
@@ -71,20 +76,10 @@ enum MFC_SHM_OFS {
         DBG_HISTORY_INPUT1      = 0xD4, /* C */
         DBG_HISTORY_OUTPUT      = 0xD8, /* C */
         HIERARCHICAL_P_QP       = 0xE0, /* E, H.264 */
+        FRAME_PACK_SEI_ENABLE   = 0x168, /* C */
+        FRAME_PACK_SEI_AVAIL    = 0x16c, /* D */
+        FRAME_PACK_SEI_INFO     = 0x17c, /* E */
 };
 
-int s5p_mfc_init_shm(struct s5p_mfc_ctx *ctx);
-
-#define s5p_mfc_write_shm(ctx, x, ofs)          \
-        do {                                    \
-                writel(x, (ctx->shm + ofs));    \
-                wmb();                          \
-        } while (0)
-
-static inline u32 s5p_mfc_read_shm(struct s5p_mfc_ctx *ctx, unsigned int ofs)
-{
-        rmb();
-        return readl(ctx->shm + ofs);
-}
-
-#endif /* S5P_MFC_SHM_H_ */
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v5(void);
+#endif /* S5P_MFC_OPR_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
new file mode 100644
index 000000000000..50b5bee3c44e
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -0,0 +1,1956 @@
+/*
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+ *
+ * Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains hw related functions.
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#undef DEBUG
+
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/firmware.h>
+#include <linux/err.h>
+#include <linux/sched.h>
+#include <linux/dma-mapping.h>
+
+#include <asm/cacheflush.h>
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_pm.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_opr_v6.h"
+
+/* #define S5P_MFC_DEBUG_REGWRITE  */
+#ifdef S5P_MFC_DEBUG_REGWRITE
+#undef writel
+#define writel(v, r)                                                    \
+        do {                                                            \
+                pr_err("MFCWRITE(%p): %08x\n", r, (unsigned int)v);     \
+        __raw_writel(v, r);                                             \
+        } while (0)
+#endif /* S5P_MFC_DEBUG_REGWRITE */
+
+#define READL(offset)           readl(dev->regs_base + (offset))
+#define WRITEL(data, offset)    writel((data), dev->regs_base + (offset))
+#define OFFSETA(x)              (((x) - dev->port_a) >> S5P_FIMV_MEM_OFFSET)
+#define OFFSETB(x)              (((x) - dev->port_b) >> S5P_FIMV_MEM_OFFSET)
+
+/* Allocate temporary buffers for decoding */
+int s5p_mfc_alloc_dec_temp_buffers_v6(struct s5p_mfc_ctx *ctx)
+{
+        /* NOP */
+
+        return 0;
+}
+
+/* Release temproary buffers for decoding */
+void s5p_mfc_release_dec_desc_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+        /* NOP */
+}
+
+int s5p_mfc_get_dec_status_v6(struct s5p_mfc_dev *dev)
+{
+        /* NOP */
+        return -1;
+}
+
+/* Allocate codec buffers */
+int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int mb_width, mb_height;
+
+        mb_width = MB_WIDTH(ctx->img_width);
+        mb_height = MB_HEIGHT(ctx->img_height);
+
+        if (ctx->type == MFCINST_DECODER) {
+                mfc_debug(2, "Luma size:%d Chroma size:%d MV size:%d\n",
+                          ctx->luma_size, ctx->chroma_size, ctx->mv_size);
+                mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
+        } else if (ctx->type == MFCINST_ENCODER) {
+                ctx->tmv_buffer_size = S5P_FIMV_NUM_TMV_BUFFERS_V6 *
+                        ALIGN(S5P_FIMV_TMV_BUFFER_SIZE_V6(mb_width, mb_height),
+                        S5P_FIMV_TMV_BUFFER_ALIGN_V6);
+                ctx->luma_dpb_size = ALIGN((mb_width * mb_height) *
+                                S5P_FIMV_LUMA_MB_TO_PIXEL_V6,
+                                S5P_FIMV_LUMA_DPB_BUFFER_ALIGN_V6);
+                ctx->chroma_dpb_size = ALIGN((mb_width * mb_height) *
+                                S5P_FIMV_CHROMA_MB_TO_PIXEL_V6,
+                                S5P_FIMV_CHROMA_DPB_BUFFER_ALIGN_V6);
+                ctx->me_buffer_size = ALIGN(S5P_FIMV_ME_BUFFER_SIZE_V6(
+                                        ctx->img_width, ctx->img_height,
+                                        mb_width, mb_height),
+                                        S5P_FIMV_ME_BUFFER_ALIGN_V6);
+
+                mfc_debug(2, "recon luma size: %d chroma size: %d\n",
+                          ctx->luma_dpb_size, ctx->chroma_dpb_size);
+        } else {
+                return -EINVAL;
+        }
+
+        /* Codecs have different memory requirements */
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_DEC:
+        case S5P_MFC_CODEC_H264_MVC_DEC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size =
+                        ctx->scratch_buf_size +
+                        (ctx->mv_count * ctx->mv_size);
+                break;
+        case S5P_MFC_CODEC_MPEG4_DEC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_DEC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size = ctx->scratch_buf_size;
+                break;
+        case S5P_MFC_CODEC_VC1RCV_DEC:
+        case S5P_MFC_CODEC_VC1_DEC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_VC1_DEC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size = ctx->scratch_buf_size;
+                break;
+        case S5P_MFC_CODEC_MPEG2_DEC:
+                ctx->bank1_size = 0;
+                ctx->bank2_size = 0;
+                break;
+        case S5P_MFC_CODEC_H263_DEC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_H263_DEC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size = ctx->scratch_buf_size;
+                break;
+        case S5P_MFC_CODEC_VP8_DEC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size = ctx->scratch_buf_size;
+                break;
+        case S5P_MFC_CODEC_H264_ENC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size =
+                        ctx->scratch_buf_size + ctx->tmv_buffer_size +
+                        (ctx->dpb_count * (ctx->luma_dpb_size +
+                        ctx->chroma_dpb_size + ctx->me_buffer_size));
+                ctx->bank2_size = 0;
+                break;
+        case S5P_MFC_CODEC_MPEG4_ENC:
+        case S5P_MFC_CODEC_H263_ENC:
+                ctx->scratch_buf_size =
+                        S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_ENC_V6(
+                                        mb_width,
+                                        mb_height);
+                ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+                                S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+                ctx->bank1_size =
+                        ctx->scratch_buf_size + ctx->tmv_buffer_size +
+                        (ctx->dpb_count * (ctx->luma_dpb_size +
+                        ctx->chroma_dpb_size + ctx->me_buffer_size));
+                ctx->bank2_size = 0;
+                break;
+        default:
+                break;
+        }
+
+        /* Allocate only if memory from bank 1 is necessary */
+        if (ctx->bank1_size > 0) {
+                ctx->bank1_buf = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_size);
+                if (IS_ERR(ctx->bank1_buf)) {
+                        ctx->bank1_buf = 0;
+                        pr_err("Buf alloc for decoding failed (port A)\n");
+                        return -ENOMEM;
+                }
+                ctx->bank1_phys = s5p_mfc_mem_cookie(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_buf);
+                BUG_ON(ctx->bank1_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+        }
+
+        return 0;
+}
+
+/* Release buffers allocated for codec */
+void s5p_mfc_release_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
+{
+        if (ctx->bank1_buf) {
+                vb2_dma_contig_memops.put(ctx->bank1_buf);
+                ctx->bank1_buf = 0;
+                ctx->bank1_phys = 0;
+                ctx->bank1_size = 0;
+        }
+}
+
+/* Allocate memory for instance data buffer */
+int s5p_mfc_alloc_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
+
+        mfc_debug_enter();
+
+        switch (ctx->codec_mode) {
+        case S5P_MFC_CODEC_H264_DEC:
+        case S5P_MFC_CODEC_H264_MVC_DEC:
+                ctx->ctx.size = buf_size->h264_dec_ctx;
+                break;
+        case S5P_MFC_CODEC_MPEG4_DEC:
+        case S5P_MFC_CODEC_H263_DEC:
+        case S5P_MFC_CODEC_VC1RCV_DEC:
+        case S5P_MFC_CODEC_VC1_DEC:
+        case S5P_MFC_CODEC_MPEG2_DEC:
+        case S5P_MFC_CODEC_VP8_DEC:
+                ctx->ctx.size = buf_size->other_dec_ctx;
+                break;
+        case S5P_MFC_CODEC_H264_ENC:
+                ctx->ctx.size = buf_size->h264_enc_ctx;
+                break;
+        case S5P_MFC_CODEC_MPEG4_ENC:
+        case S5P_MFC_CODEC_H263_ENC:
+                ctx->ctx.size = buf_size->other_enc_ctx;
+                break;
+        default:
+                ctx->ctx.size = 0;
+                mfc_err("Codec type(%d) should be checked!\n", ctx->codec_mode);
+                break;
+        }
+
+        ctx->ctx.alloc = vb2_dma_contig_memops.alloc(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx.size);
+        if (IS_ERR(ctx->ctx.alloc)) {
+                mfc_err("Allocating context buffer failed.\n");
+                return PTR_ERR(ctx->ctx.alloc);
+        }
+
+        ctx->ctx.dma = s5p_mfc_mem_cookie(
+                dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx.alloc);
+
+        ctx->ctx.virt = vb2_dma_contig_memops.vaddr(ctx->ctx.alloc);
+        if (!ctx->ctx.virt) {
+                vb2_dma_contig_memops.put(ctx->ctx.alloc);
+                ctx->ctx.alloc = NULL;
+                ctx->ctx.dma = 0;
+                ctx->ctx.virt = NULL;
+
+                mfc_err("Remapping context buffer failed.\n");
+                return -ENOMEM;
+        }
+
+        memset(ctx->ctx.virt, 0, ctx->ctx.size);
+        wmb();
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+/* Release instance buffer */
+void s5p_mfc_release_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+        mfc_debug_enter();
+
+        if (ctx->ctx.alloc) {
+                vb2_dma_contig_memops.put(ctx->ctx.alloc);
+                ctx->ctx.alloc = NULL;
+                ctx->ctx.dma = 0;
+                ctx->ctx.virt = NULL;
+        }
+
+        mfc_debug_leave();
+}
+
+/* Allocate context buffers for SYS_INIT */
+int s5p_mfc_alloc_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
+
+        mfc_debug_enter();
+
+        dev->ctx_buf.alloc = vb2_dma_contig_memops.alloc(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], buf_size->dev_ctx);
+        if (IS_ERR(dev->ctx_buf.alloc)) {
+                mfc_err("Allocating DESC buffer failed.\n");
+                return PTR_ERR(dev->ctx_buf.alloc);
+        }
+
+        dev->ctx_buf.dma = s5p_mfc_mem_cookie(
+                        dev->alloc_ctx[MFC_BANK1_ALLOC_CTX],
+                        dev->ctx_buf.alloc);
+
+        dev->ctx_buf.virt = vb2_dma_contig_memops.vaddr(dev->ctx_buf.alloc);
+        if (!dev->ctx_buf.virt) {
+                vb2_dma_contig_memops.put(dev->ctx_buf.alloc);
+                dev->ctx_buf.alloc = NULL;
+                dev->ctx_buf.dma = 0;
+
+                mfc_err("Remapping DESC buffer failed.\n");
+                return -ENOMEM;
+        }
+
+        memset(dev->ctx_buf.virt, 0, buf_size->dev_ctx);
+        wmb();
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+/* Release context buffers for SYS_INIT */
+void s5p_mfc_release_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
+{
+        if (dev->ctx_buf.alloc) {
+                vb2_dma_contig_memops.put(dev->ctx_buf.alloc);
+                dev->ctx_buf.alloc = NULL;
+                dev->ctx_buf.dma = 0;
+                dev->ctx_buf.virt = NULL;
+        }
+}
+
+static int calc_plane(int width, int height)
+{
+        int mbX, mbY;
+
+        mbX = DIV_ROUND_UP(width, S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6);
+        mbY = DIV_ROUND_UP(height, S5P_FIMV_NUM_PIXELS_IN_MB_COL_V6);
+
+        if (width * height < S5P_FIMV_MAX_FRAME_SIZE_V6)
+                mbY = (mbY + 1) / 2 * 2;
+
+        return (mbX * S5P_FIMV_NUM_PIXELS_IN_MB_COL_V6) *
+                (mbY * S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6);
+}
+
+void s5p_mfc_dec_calc_dpb_size_v6(struct s5p_mfc_ctx *ctx)
+{
+        ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN_V6);
+        ctx->buf_height = ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN_V6);
+        mfc_debug(2, "SEQ Done: Movie dimensions %dx%d,\n"
+                        "buffer dimensions: %dx%d\n", ctx->img_width,
+                        ctx->img_height, ctx->buf_width, ctx->buf_height);
+
+        ctx->luma_size = calc_plane(ctx->img_width, ctx->img_height);
+        ctx->chroma_size = calc_plane(ctx->img_width, (ctx->img_height >> 1));
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+                        ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) {
+                ctx->mv_size = S5P_MFC_DEC_MV_SIZE_V6(ctx->img_width,
+                                ctx->img_height);
+                ctx->mv_size = ALIGN(ctx->mv_size, 16);
+        } else {
+                ctx->mv_size = 0;
+        }
+}
+
+void s5p_mfc_enc_calc_src_size_v6(struct s5p_mfc_ctx *ctx)
+{
+        unsigned int mb_width, mb_height;
+
+        mb_width = MB_WIDTH(ctx->img_width);
+        mb_height = MB_HEIGHT(ctx->img_height);
+
+        ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN_V6);
+        ctx->luma_size = ALIGN((mb_width * mb_height) * 256, 256);
+        ctx->chroma_size = ALIGN((mb_width * mb_height) * 128, 256);
+}
+
+/* Set registers for decoding stream buffer */
+int s5p_mfc_set_dec_stream_buffer_v6(struct s5p_mfc_ctx *ctx, int buf_addr,
+                  unsigned int start_num_byte, unsigned int strm_size)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf_size *buf_size = dev->variant->buf_size;
+
+        mfc_debug_enter();
+        mfc_debug(2, "inst_no: %d, buf_addr: 0x%08x,\n"
+                "buf_size: 0x%08x (%d)\n",
+                ctx->inst_no, buf_addr, strm_size, strm_size);
+        WRITEL(strm_size, S5P_FIMV_D_STREAM_DATA_SIZE_V6);
+        WRITEL(buf_addr, S5P_FIMV_D_CPB_BUFFER_ADDR_V6);
+        WRITEL(buf_size->cpb, S5P_FIMV_D_CPB_BUFFER_SIZE_V6);
+        WRITEL(start_num_byte, S5P_FIMV_D_CPB_BUFFER_OFFSET_V6);
+
+        mfc_debug_leave();
+        return 0;
+}
+
+/* Set decoding frame buffer */
+int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+        unsigned int frame_size, i;
+        unsigned int frame_size_ch, frame_size_mv;
+        struct s5p_mfc_dev *dev = ctx->dev;
+        size_t buf_addr1;
+        int buf_size1;
+        int align_gap;
+
+        buf_addr1 = ctx->bank1_phys;
+        buf_size1 = ctx->bank1_size;
+
+        mfc_debug(2, "Buf1: %p (%d)\n", (void *)buf_addr1, buf_size1);
+        mfc_debug(2, "Total DPB COUNT: %d\n", ctx->total_dpb_count);
+        mfc_debug(2, "Setting display delay to %d\n", ctx->display_delay);
+
+        WRITEL(ctx->total_dpb_count, S5P_FIMV_D_NUM_DPB_V6);
+        WRITEL(ctx->luma_size, S5P_FIMV_D_LUMA_DPB_SIZE_V6);
+        WRITEL(ctx->chroma_size, S5P_FIMV_D_CHROMA_DPB_SIZE_V6);
+
+        WRITEL(buf_addr1, S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V6);
+        WRITEL(ctx->scratch_buf_size, S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V6);
+        buf_addr1 += ctx->scratch_buf_size;
+        buf_size1 -= ctx->scratch_buf_size;
+
+        if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC ||
+                        ctx->codec_mode == S5P_FIMV_CODEC_H264_MVC_DEC){
+                WRITEL(ctx->mv_size, S5P_FIMV_D_MV_BUFFER_SIZE_V6);
+                WRITEL(ctx->mv_count, S5P_FIMV_D_NUM_MV_V6);
+        }
+
+        frame_size = ctx->luma_size;
+        frame_size_ch = ctx->chroma_size;
+        frame_size_mv = ctx->mv_size;
+        mfc_debug(2, "Frame size: %d ch: %d mv: %d\n",
+                        frame_size, frame_size_ch, frame_size_mv);
+
+        for (i = 0; i < ctx->total_dpb_count; i++) {
+                /* Bank2 */
+                mfc_debug(2, "Luma %d: %x\n", i,
+                                        ctx->dst_bufs[i].cookie.raw.luma);
+                WRITEL(ctx->dst_bufs[i].cookie.raw.luma,
+                                S5P_FIMV_D_LUMA_DPB_V6 + i * 4);
+                mfc_debug(2, "\tChroma %d: %x\n", i,
+                                        ctx->dst_bufs[i].cookie.raw.chroma);
+                WRITEL(ctx->dst_bufs[i].cookie.raw.chroma,
+                                S5P_FIMV_D_CHROMA_DPB_V6 + i * 4);
+        }
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+                        ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) {
+                for (i = 0; i < ctx->mv_count; i++) {
+                        /* To test alignment */
+                        align_gap = buf_addr1;
+                        buf_addr1 = ALIGN(buf_addr1, 16);
+                        align_gap = buf_addr1 - align_gap;
+                        buf_size1 -= align_gap;
+
+                        mfc_debug(2, "\tBuf1: %x, size: %d\n",
+                                        buf_addr1, buf_size1);
+                        WRITEL(buf_addr1, S5P_FIMV_D_MV_BUFFER_V6 + i * 4);
+                        buf_addr1 += frame_size_mv;
+                        buf_size1 -= frame_size_mv;
+                }
+        }
+
+        mfc_debug(2, "Buf1: %u, buf_size1: %d (frames %d)\n",
+                        buf_addr1, buf_size1, ctx->total_dpb_count);
+        if (buf_size1 < 0) {
+                mfc_debug(2, "Not enough memory has been allocated.\n");
+                return -ENOMEM;
+        }
+
+        WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+        s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                        S5P_FIMV_CH_INIT_BUFS_V6, NULL);
+
+        mfc_debug(2, "After setting buffers.\n");
+        return 0;
+}
+
+/* Set registers for encoding stream buffer */
+int s5p_mfc_set_enc_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
+                unsigned long addr, unsigned int size)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        WRITEL(addr, S5P_FIMV_E_STREAM_BUFFER_ADDR_V6); /* 16B align */
+        WRITEL(size, S5P_FIMV_E_STREAM_BUFFER_SIZE_V6);
+
+        mfc_debug(2, "stream buf addr: 0x%08lx, size: 0x%d",
+                addr, size);
+
+        return 0;
+}
+
+void s5p_mfc_set_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
+                unsigned long y_addr, unsigned long c_addr)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        WRITEL(y_addr, S5P_FIMV_E_SOURCE_LUMA_ADDR_V6); /* 256B align */
+        WRITEL(c_addr, S5P_FIMV_E_SOURCE_CHROMA_ADDR_V6);
+
+        mfc_debug(2, "enc src y buf addr: 0x%08lx", y_addr);
+        mfc_debug(2, "enc src c buf addr: 0x%08lx", c_addr);
+}
+
+void s5p_mfc_get_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
+                unsigned long *y_addr, unsigned long *c_addr)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long enc_recon_y_addr, enc_recon_c_addr;
+
+        *y_addr = READL(S5P_FIMV_E_ENCODED_SOURCE_LUMA_ADDR_V6);
+        *c_addr = READL(S5P_FIMV_E_ENCODED_SOURCE_CHROMA_ADDR_V6);
+
+        enc_recon_y_addr = READL(S5P_FIMV_E_RECON_LUMA_DPB_ADDR_V6);
+        enc_recon_c_addr = READL(S5P_FIMV_E_RECON_CHROMA_DPB_ADDR_V6);
+
+        mfc_debug(2, "recon y addr: 0x%08lx", enc_recon_y_addr);
+        mfc_debug(2, "recon c addr: 0x%08lx", enc_recon_c_addr);
+}
+
+/* Set encoding ref & codec buffer */
+int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        size_t buf_addr1, buf_size1;
+        int i;
+
+        mfc_debug_enter();
+
+        buf_addr1 = ctx->bank1_phys;
+        buf_size1 = ctx->bank1_size;
+
+        mfc_debug(2, "Buf1: %p (%d)\n", (void *)buf_addr1, buf_size1);
+
+        for (i = 0; i < ctx->dpb_count; i++) {
+                WRITEL(buf_addr1, S5P_FIMV_E_LUMA_DPB_V6 + (4 * i));
+                buf_addr1 += ctx->luma_dpb_size;
+                WRITEL(buf_addr1, S5P_FIMV_E_CHROMA_DPB_V6 + (4 * i));
+                buf_addr1 += ctx->chroma_dpb_size;
+                WRITEL(buf_addr1, S5P_FIMV_E_ME_BUFFER_V6 + (4 * i));
+                buf_addr1 += ctx->me_buffer_size;
+                buf_size1 -= (ctx->luma_dpb_size + ctx->chroma_dpb_size +
+                        ctx->me_buffer_size);
+        }
+
+        WRITEL(buf_addr1, S5P_FIMV_E_SCRATCH_BUFFER_ADDR_V6);
+        WRITEL(ctx->scratch_buf_size, S5P_FIMV_E_SCRATCH_BUFFER_SIZE_V6);
+        buf_addr1 += ctx->scratch_buf_size;
+        buf_size1 -= ctx->scratch_buf_size;
+
+        WRITEL(buf_addr1, S5P_FIMV_E_TMV_BUFFER0_V6);
+        buf_addr1 += ctx->tmv_buffer_size >> 1;
+        WRITEL(buf_addr1, S5P_FIMV_E_TMV_BUFFER1_V6);
+        buf_addr1 += ctx->tmv_buffer_size >> 1;
+        buf_size1 -= ctx->tmv_buffer_size;
+
+        mfc_debug(2, "Buf1: %u, buf_size1: %d (ref frames %d)\n",
+                        buf_addr1, buf_size1, ctx->dpb_count);
+        if (buf_size1 < 0) {
+                mfc_debug(2, "Not enough memory has been allocated.\n");
+                return -ENOMEM;
+        }
+
+        WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+        s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                        S5P_FIMV_CH_INIT_BUFS_V6, NULL);
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+static int s5p_mfc_set_slice_mode(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        /* multi-slice control */
+        /* multi-slice MB number or bit size */
+        WRITEL(ctx->slice_mode, S5P_FIMV_E_MSLICE_MODE_V6);
+        if (ctx->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+                WRITEL(ctx->slice_size.mb, S5P_FIMV_E_MSLICE_SIZE_MB_V6);
+        } else if (ctx->slice_mode ==
+                        V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+                WRITEL(ctx->slice_size.bits, S5P_FIMV_E_MSLICE_SIZE_BITS_V6);
+        } else {
+                WRITEL(0x0, S5P_FIMV_E_MSLICE_SIZE_MB_V6);
+                WRITEL(0x0, S5P_FIMV_E_MSLICE_SIZE_BITS_V6);
+        }
+
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        unsigned int reg = 0;
+
+        mfc_debug_enter();
+
+        /* width */
+        WRITEL(ctx->img_width, S5P_FIMV_E_FRAME_WIDTH_V6); /* 16 align */
+        /* height */
+        WRITEL(ctx->img_height, S5P_FIMV_E_FRAME_HEIGHT_V6); /* 16 align */
+
+        /* cropped width */
+        WRITEL(ctx->img_width, S5P_FIMV_E_CROPPED_FRAME_WIDTH_V6);
+        /* cropped height */
+        WRITEL(ctx->img_height, S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6);
+        /* cropped offset */
+        WRITEL(0x0, S5P_FIMV_E_FRAME_CROP_OFFSET_V6);
+
+        /* pictype : IDR period */
+        reg = 0;
+        reg |= p->gop_size & 0xFFFF;
+        WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+
+        /* multi-slice control */
+        /* multi-slice MB number or bit size */
+        ctx->slice_mode = p->slice_mode;
+        reg = 0;
+        if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+                reg |= (0x1 << 3);
+                WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+                ctx->slice_size.mb = p->slice_mb;
+        } else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+                reg |= (0x1 << 3);
+                WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+                ctx->slice_size.bits = p->slice_bit;
+        } else {
+                reg &= ~(0x1 << 3);
+                WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+        }
+
+        s5p_mfc_set_slice_mode(ctx);
+
+        /* cyclic intra refresh */
+        WRITEL(p->intra_refresh_mb, S5P_FIMV_E_IR_SIZE_V6);
+        reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+        if (p->intra_refresh_mb == 0)
+                reg &= ~(0x1 << 4);
+        else
+                reg |= (0x1 << 4);
+        WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+
+        /* 'NON_REFERENCE_STORE_ENABLE' for debugging */
+        reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+        reg &= ~(0x1 << 9);
+        WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+
+        /* memory structure cur. frame */
+        if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
+                /* 0: Linear, 1: 2D tiled*/
+                reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+                reg &= ~(0x1 << 7);
+                WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+                /* 0: NV12(CbCr), 1: NV21(CrCb) */
+                WRITEL(0x0, S5P_FIMV_PIXEL_FORMAT_V6);
+        } else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV21M) {
+                /* 0: Linear, 1: 2D tiled*/
+                reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+                reg &= ~(0x1 << 7);
+                WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+                /* 0: NV12(CbCr), 1: NV21(CrCb) */
+                WRITEL(0x1, S5P_FIMV_PIXEL_FORMAT_V6);
+        } else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16) {
+                /* 0: Linear, 1: 2D tiled*/
+                reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+                reg |= (0x1 << 7);
+                WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+                /* 0: NV12(CbCr), 1: NV21(CrCb) */
+                WRITEL(0x0, S5P_FIMV_PIXEL_FORMAT_V6);
+        }
+
+        /* memory structure recon. frame */
+        /* 0: Linear, 1: 2D tiled */
+        reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+        reg |= (0x1 << 8);
+        WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+
+        /* padding control & value */
+        WRITEL(0x0, S5P_FIMV_E_PADDING_CTRL_V6);
+        if (p->pad) {
+                reg = 0;
+                /** enable */
+                reg |= (1 << 31);
+                /** cr value */
+                reg |= ((p->pad_cr & 0xFF) << 16);
+                /** cb value */
+                reg |= ((p->pad_cb & 0xFF) << 8);
+                /** y value */
+                reg |= p->pad_luma & 0xFF;
+                WRITEL(reg, S5P_FIMV_E_PADDING_CTRL_V6);
+        }
+
+        /* rate control config. */
+        reg = 0;
+        /* frame-level rate control */
+        reg |= ((p->rc_frame & 0x1) << 9);
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+        /* bit rate */
+        if (p->rc_frame)
+                WRITEL(p->rc_bitrate,
+                        S5P_FIMV_E_RC_BIT_RATE_V6);
+        else
+                WRITEL(1, S5P_FIMV_E_RC_BIT_RATE_V6);
+
+        /* reaction coefficient */
+        if (p->rc_frame) {
+                if (p->rc_reaction_coeff < TIGHT_CBR_MAX) /* tight CBR */
+                        WRITEL(1, S5P_FIMV_E_RC_RPARAM_V6);
+                else                                      /* loose CBR */
+                        WRITEL(2, S5P_FIMV_E_RC_RPARAM_V6);
+        }
+
+        /* seq header ctrl */
+        reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+        reg &= ~(0x1 << 2);
+        reg |= ((p->seq_hdr_mode & 0x1) << 2);
+
+        /* frame skip mode */
+        reg &= ~(0x3);
+        reg |= (p->frame_skip_mode & 0x3);
+        WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+
+        /* 'DROP_CONTROL_ENABLE', disable */
+        reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+        reg &= ~(0x1 << 10);
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+        /* setting for MV range [16, 256] */
+        reg = 0;
+        reg &= ~(0x3FFF);
+        reg = 256;
+        WRITEL(reg, S5P_FIMV_E_MV_HOR_RANGE_V6);
+
+        reg = 0;
+        reg &= ~(0x3FFF);
+        reg = 256;
+        WRITEL(reg, S5P_FIMV_E_MV_VER_RANGE_V6);
+
+        WRITEL(0x0, S5P_FIMV_E_FRAME_INSERTION_V6);
+        WRITEL(0x0, S5P_FIMV_E_ROI_BUFFER_ADDR_V6);
+        WRITEL(0x0, S5P_FIMV_E_PARAM_CHANGE_V6);
+        WRITEL(0x0, S5P_FIMV_E_RC_ROI_CTRL_V6);
+        WRITEL(0x0, S5P_FIMV_E_PICTURE_TAG_V6);
+
+        WRITEL(0x0, S5P_FIMV_E_BIT_COUNT_ENABLE_V6);
+        WRITEL(0x0, S5P_FIMV_E_MAX_BIT_COUNT_V6);
+        WRITEL(0x0, S5P_FIMV_E_MIN_BIT_COUNT_V6);
+
+        WRITEL(0x0, S5P_FIMV_E_METADATA_BUFFER_ADDR_V6);
+        WRITEL(0x0, S5P_FIMV_E_METADATA_BUFFER_SIZE_V6);
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_h264_enc_params *p_h264 = &p->codec.h264;
+        unsigned int reg = 0;
+        int i;
+
+        mfc_debug_enter();
+
+        s5p_mfc_set_enc_params(ctx);
+
+        /* pictype : number of B */
+        reg = READL(S5P_FIMV_E_GOP_CONFIG_V6);
+        reg &= ~(0x3 << 16);
+        reg |= ((p->num_b_frame & 0x3) << 16);
+        WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+
+        /* profile & level */
+        reg = 0;
+        /** level */
+        reg |= ((p_h264->level & 0xFF) << 8);
+        /** profile - 0 ~ 3 */
+        reg |= p_h264->profile & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_PICTURE_PROFILE_V6);
+
+        /* rate control config. */
+        reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+        /** macroblock level rate control */
+        reg &= ~(0x1 << 8);
+        reg |= ((p->rc_mb & 0x1) << 8);
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+        /** frame QP */
+        reg &= ~(0x3F);
+        reg |= p_h264->rc_frame_qp & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+        /* max & min value of QP */
+        reg = 0;
+        /** max QP */
+        reg |= ((p_h264->rc_max_qp & 0x3F) << 8);
+        /** min QP */
+        reg |= p_h264->rc_min_qp & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_RC_QP_BOUND_V6);
+
+        /* other QPs */
+        WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+        if (!p->rc_frame && !p->rc_mb) {
+                reg = 0;
+                reg |= ((p_h264->rc_b_frame_qp & 0x3F) << 16);
+                reg |= ((p_h264->rc_p_frame_qp & 0x3F) << 8);
+                reg |= p_h264->rc_frame_qp & 0x3F;
+                WRITEL(reg, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+        }
+
+        /* frame rate */
+        if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+                reg = 0;
+                reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+                reg |= p->rc_framerate_denom & 0xFFFF;
+                WRITEL(reg, S5P_FIMV_E_RC_FRAME_RATE_V6);
+        }
+
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                WRITEL(p_h264->cpb_size & 0xFFFF,
+                                S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+
+                if (p->rc_frame)
+                        WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+        }
+
+        /* interlace */
+        reg = 0;
+        reg |= ((p_h264->interlace & 0x1) << 3);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* height */
+        if (p_h264->interlace) {
+                WRITEL(ctx->img_height >> 1,
+                                S5P_FIMV_E_FRAME_HEIGHT_V6); /* 32 align */
+                /* cropped height */
+                WRITEL(ctx->img_height >> 1,
+                                S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6);
+        }
+
+        /* loop filter ctrl */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x3 << 1);
+        reg |= ((p_h264->loop_filter_mode & 0x3) << 1);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* loopfilter alpha offset */
+        if (p_h264->loop_filter_alpha < 0) {
+                reg = 0x10;
+                reg |= (0xFF - p_h264->loop_filter_alpha) + 1;
+        } else {
+                reg = 0x00;
+                reg |= (p_h264->loop_filter_alpha & 0xF);
+        }
+        WRITEL(reg, S5P_FIMV_E_H264_LF_ALPHA_OFFSET_V6);
+
+        /* loopfilter beta offset */
+        if (p_h264->loop_filter_beta < 0) {
+                reg = 0x10;
+                reg |= (0xFF - p_h264->loop_filter_beta) + 1;
+        } else {
+                reg = 0x00;
+                reg |= (p_h264->loop_filter_beta & 0xF);
+        }
+        WRITEL(reg, S5P_FIMV_E_H264_LF_BETA_OFFSET_V6);
+
+        /* entropy coding mode */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1);
+        reg |= p_h264->entropy_mode & 0x1;
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* number of ref. picture */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 7);
+        reg |= (((p_h264->num_ref_pic_4p - 1) & 0x1) << 7);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* 8x8 transform enable */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x3 << 12);
+        reg |= ((p_h264->_8x8_transform & 0x3) << 12);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* macroblock adaptive scaling features */
+        WRITEL(0x0, S5P_FIMV_E_MB_RC_CONFIG_V6);
+        if (p->rc_mb) {
+                reg = 0;
+                /** dark region */
+                reg |= ((p_h264->rc_mb_dark & 0x1) << 3);
+                /** smooth region */
+                reg |= ((p_h264->rc_mb_smooth & 0x1) << 2);
+                /** static region */
+                reg |= ((p_h264->rc_mb_static & 0x1) << 1);
+                /** high activity region */
+                reg |= p_h264->rc_mb_activity & 0x1;
+                WRITEL(reg, S5P_FIMV_E_MB_RC_CONFIG_V6);
+        }
+
+        /* aspect ratio VUI */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 5);
+        reg |= ((p_h264->vui_sar & 0x1) << 5);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        WRITEL(0x0, S5P_FIMV_E_ASPECT_RATIO_V6);
+        WRITEL(0x0, S5P_FIMV_E_EXTENDED_SAR_V6);
+        if (p_h264->vui_sar) {
+                /* aspect ration IDC */
+                reg = 0;
+                reg |= p_h264->vui_sar_idc & 0xFF;
+                WRITEL(reg, S5P_FIMV_E_ASPECT_RATIO_V6);
+                if (p_h264->vui_sar_idc == 0xFF) {
+                        /* extended SAR */
+                        reg = 0;
+                        reg |= (p_h264->vui_ext_sar_width & 0xFFFF) << 16;
+                        reg |= p_h264->vui_ext_sar_height & 0xFFFF;
+                        WRITEL(reg, S5P_FIMV_E_EXTENDED_SAR_V6);
+                }
+        }
+
+        /* intra picture period for H.264 open GOP */
+        /* control */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 4);
+        reg |= ((p_h264->open_gop & 0x1) << 4);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+        /* value */
+        WRITEL(0x0, S5P_FIMV_E_H264_I_PERIOD_V6);
+        if (p_h264->open_gop) {
+                reg = 0;
+                reg |= p_h264->open_gop_size & 0xFFFF;
+                WRITEL(reg, S5P_FIMV_E_H264_I_PERIOD_V6);
+        }
+
+        /* 'WEIGHTED_BI_PREDICTION' for B is disable */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x3 << 9);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* 'CONSTRAINED_INTRA_PRED_ENABLE' is disable */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 14);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* ASO */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 6);
+        reg |= ((p_h264->aso & 0x1) << 6);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+        /* hier qp enable */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 8);
+        reg |= ((p_h264->open_gop & 0x1) << 8);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+        reg = 0;
+        if (p_h264->hier_qp && p_h264->hier_qp_layer) {
+                reg |= (p_h264->hier_qp_type & 0x1) << 0x3;
+                reg |= p_h264->hier_qp_layer & 0x7;
+                WRITEL(reg, S5P_FIMV_E_H264_NUM_T_LAYER_V6);
+                /* QP value for each layer */
+                for (i = 0; i < (p_h264->hier_qp_layer & 0x7); i++)
+                        WRITEL(p_h264->hier_qp_layer_qp[i],
+                                S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER0_V6 +
+                                i * 4);
+        }
+        /* number of coding layer should be zero when hierarchical is disable */
+        WRITEL(reg, S5P_FIMV_E_H264_NUM_T_LAYER_V6);
+
+        /* frame packing SEI generation */
+        reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+        reg &= ~(0x1 << 25);
+        reg |= ((p_h264->sei_frame_packing & 0x1) << 25);
+        WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+        if (p_h264->sei_frame_packing) {
+                reg = 0;
+                /** current frame0 flag */
+                reg |= ((p_h264->sei_fp_curr_frame_0 & 0x1) << 2);
+                /** arrangement type */
+                reg |= p_h264->sei_fp_arrangement_type & 0x3;
+                WRITEL(reg, S5P_FIMV_E_H264_FRAME_PACKING_SEI_INFO_V6);
+        }
+
+        if (p_h264->fmo) {
+                switch (p_h264->fmo_map_type) {
+                case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_INTERLEAVED_SLICES:
+                        if (p_h264->fmo_slice_grp > 4)
+                                p_h264->fmo_slice_grp = 4;
+                        for (i = 0; i < (p_h264->fmo_slice_grp & 0xF); i++)
+                                WRITEL(p_h264->fmo_run_len[i] - 1,
+                                S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_0_V6 +
+                                i * 4);
+                        break;
+                case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_SCATTERED_SLICES:
+                        if (p_h264->fmo_slice_grp > 4)
+                                p_h264->fmo_slice_grp = 4;
+                        break;
+                case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_RASTER_SCAN:
+                case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_WIPE_SCAN:
+                        if (p_h264->fmo_slice_grp > 2)
+                                p_h264->fmo_slice_grp = 2;
+                        WRITEL(p_h264->fmo_chg_dir & 0x1,
+                                S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_DIR_V6);
+                        /* the valid range is 0 ~ number of macroblocks -1 */
+                        WRITEL(p_h264->fmo_chg_rate,
+                                S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_RATE_MINUS1_V6);
+                        break;
+                default:
+                        mfc_err("Unsupported map type for FMO: %d\n",
+                                        p_h264->fmo_map_type);
+                        p_h264->fmo_map_type = 0;
+                        p_h264->fmo_slice_grp = 1;
+                        break;
+                }
+
+                WRITEL(p_h264->fmo_map_type,
+                                S5P_FIMV_E_H264_FMO_SLICE_GRP_MAP_TYPE_V6);
+                WRITEL(p_h264->fmo_slice_grp - 1,
+                                S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6);
+        } else {
+                WRITEL(0, S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6);
+        }
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
+        unsigned int reg = 0;
+
+        mfc_debug_enter();
+
+        s5p_mfc_set_enc_params(ctx);
+
+        /* pictype : number of B */
+        reg = READL(S5P_FIMV_E_GOP_CONFIG_V6);
+        reg &= ~(0x3 << 16);
+        reg |= ((p->num_b_frame & 0x3) << 16);
+        WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+
+        /* profile & level */
+        reg = 0;
+        /** level */
+        reg |= ((p_mpeg4->level & 0xFF) << 8);
+        /** profile - 0 ~ 1 */
+        reg |= p_mpeg4->profile & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_PICTURE_PROFILE_V6);
+
+        /* rate control config. */
+        reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+        /** macroblock level rate control */
+        reg &= ~(0x1 << 8);
+        reg |= ((p->rc_mb & 0x1) << 8);
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+        /** frame QP */
+        reg &= ~(0x3F);
+        reg |= p_mpeg4->rc_frame_qp & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+        /* max & min value of QP */
+        reg = 0;
+        /** max QP */
+        reg |= ((p_mpeg4->rc_max_qp & 0x3F) << 8);
+        /** min QP */
+        reg |= p_mpeg4->rc_min_qp & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_RC_QP_BOUND_V6);
+
+        /* other QPs */
+        WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+        if (!p->rc_frame && !p->rc_mb) {
+                reg = 0;
+                reg |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 16);
+                reg |= ((p_mpeg4->rc_p_frame_qp & 0x3F) << 8);
+                reg |= p_mpeg4->rc_frame_qp & 0x3F;
+                WRITEL(reg, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+        }
+
+        /* frame rate */
+        if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+                reg = 0;
+                reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+                reg |= p->rc_framerate_denom & 0xFFFF;
+                WRITEL(reg, S5P_FIMV_E_RC_FRAME_RATE_V6);
+        }
+
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                WRITEL(p->vbv_size & 0xFFFF, S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+
+                if (p->rc_frame)
+                        WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+        }
+
+        /* Disable HEC */
+        WRITEL(0x0, S5P_FIMV_E_MPEG4_OPTIONS_V6);
+        WRITEL(0x0, S5P_FIMV_E_MPEG4_HEC_PERIOD_V6);
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
+        unsigned int reg = 0;
+
+        mfc_debug_enter();
+
+        s5p_mfc_set_enc_params(ctx);
+
+        /* profile & level */
+        reg = 0;
+        /** profile */
+        reg |= (0x1 << 4);
+        WRITEL(reg, S5P_FIMV_E_PICTURE_PROFILE_V6);
+
+        /* rate control config. */
+        reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+        /** macroblock level rate control */
+        reg &= ~(0x1 << 8);
+        reg |= ((p->rc_mb & 0x1) << 8);
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+        /** frame QP */
+        reg &= ~(0x3F);
+        reg |= p_h263->rc_frame_qp & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+        /* max & min value of QP */
+        reg = 0;
+        /** max QP */
+        reg |= ((p_h263->rc_max_qp & 0x3F) << 8);
+        /** min QP */
+        reg |= p_h263->rc_min_qp & 0x3F;
+        WRITEL(reg, S5P_FIMV_E_RC_QP_BOUND_V6);
+
+        /* other QPs */
+        WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+        if (!p->rc_frame && !p->rc_mb) {
+                reg = 0;
+                reg |= ((p_h263->rc_b_frame_qp & 0x3F) << 16);
+                reg |= ((p_h263->rc_p_frame_qp & 0x3F) << 8);
+                reg |= p_h263->rc_frame_qp & 0x3F;
+                WRITEL(reg, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+        }
+
+        /* frame rate */
+        if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+                reg = 0;
+                reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+                reg |= p->rc_framerate_denom & 0xFFFF;
+                WRITEL(reg, S5P_FIMV_E_RC_FRAME_RATE_V6);
+        }
+
+        /* vbv buffer size */
+        if (p->frame_skip_mode ==
+                        V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+                WRITEL(p->vbv_size & 0xFFFF, S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+
+                if (p->rc_frame)
+                        WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+        }
+
+        mfc_debug_leave();
+
+        return 0;
+}
+
+/* Initialize decoding */
+int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int reg = 0;
+        int fmo_aso_ctrl = 0;
+
+        mfc_debug_enter();
+        mfc_debug(2, "InstNo: %d/%d\n", ctx->inst_no,
+                        S5P_FIMV_CH_SEQ_HEADER_V6);
+        mfc_debug(2, "BUFs: %08x %08x %08x\n",
+                  READL(S5P_FIMV_D_CPB_BUFFER_ADDR_V6),
+                  READL(S5P_FIMV_D_CPB_BUFFER_ADDR_V6),
+                  READL(S5P_FIMV_D_CPB_BUFFER_ADDR_V6));
+
+        /* FMO_ASO_CTRL - 0: Enable, 1: Disable */
+        reg |= (fmo_aso_ctrl << S5P_FIMV_D_OPT_FMO_ASO_CTRL_MASK_V6);
+
+        /* When user sets desplay_delay to 0,
+         * It works as "display_delay enable" and delay set to 0.
+         * If user wants display_delay disable, It should be
+         * set to negative value. */
+        if (ctx->display_delay >= 0) {
+                reg |= (0x1 << S5P_FIMV_D_OPT_DDELAY_EN_SHIFT_V6);
+                WRITEL(ctx->display_delay, S5P_FIMV_D_DISPLAY_DELAY_V6);
+        }
+        /* Setup loop filter, for decoding this is only valid for MPEG4 */
+        if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_DEC) {
+                mfc_debug(2, "Set loop filter to: %d\n",
+                                ctx->loop_filter_mpeg4);
+                reg |= (ctx->loop_filter_mpeg4 <<
+                                S5P_FIMV_D_OPT_LF_CTRL_SHIFT_V6);
+        }
+        if (ctx->dst_fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16)
+                reg |= (0x1 << S5P_FIMV_D_OPT_TILE_MODE_SHIFT_V6);
+
+        WRITEL(reg, S5P_FIMV_D_DEC_OPTIONS_V6);
+
+        /* 0: NV12(CbCr), 1: NV21(CrCb) */
+        if (ctx->dst_fmt->fourcc == V4L2_PIX_FMT_NV21M)
+                WRITEL(0x1, S5P_FIMV_PIXEL_FORMAT_V6);
+        else
+                WRITEL(0x0, S5P_FIMV_PIXEL_FORMAT_V6);
+
+        /* sei parse */
+        WRITEL(ctx->sei_fp_parse & 0x1, S5P_FIMV_D_SEI_ENABLE_V6);
+
+        WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+        s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                        S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
+
+        mfc_debug_leave();
+        return 0;
+}
+
+static inline void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned int dpb;
+        if (flush)
+                dpb = READL(S5P_FIMV_SI_CH0_DPB_CONF_CTRL) | (1 << 14);
+        else
+                dpb = READL(S5P_FIMV_SI_CH0_DPB_CONF_CTRL) & ~(1 << 14);
+        WRITEL(dpb, S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+}
+
+/* Decode a single frame */
+int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
+                        enum s5p_mfc_decode_arg last_frame)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        WRITEL(ctx->dec_dst_flag, S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V6);
+        WRITEL(ctx->slice_interface & 0x1, S5P_FIMV_D_SLICE_IF_ENABLE_V6);
+
+        WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+        /* Issue different commands to instance basing on whether it
+         * is the last frame or not. */
+        switch (last_frame) {
+        case 0:
+                s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                                S5P_FIMV_CH_FRAME_START_V6, NULL);
+                break;
+        case 1:
+                s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                                S5P_FIMV_CH_LAST_FRAME_V6, NULL);
+                break;
+        default:
+                mfc_err("Unsupported last frame arg.\n");
+                return -EINVAL;
+        }
+
+        mfc_debug(2, "Decoding a usual frame.\n");
+        return 0;
+}
+
+int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+                s5p_mfc_set_enc_params_h264(ctx);
+        else if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_ENC)
+                s5p_mfc_set_enc_params_mpeg4(ctx);
+        else if (ctx->codec_mode == S5P_MFC_CODEC_H263_ENC)
+                s5p_mfc_set_enc_params_h263(ctx);
+        else {
+                mfc_err("Unknown codec for encoding (%x).\n",
+                        ctx->codec_mode);
+                return -EINVAL;
+        }
+
+        WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+        s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                        S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
+
+        return 0;
+}
+
+int s5p_mfc_h264_set_aso_slice_order_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_enc_params *p = &ctx->enc_params;
+        struct s5p_mfc_h264_enc_params *p_h264 = &p->codec.h264;
+        int i;
+
+        if (p_h264->aso) {
+                for (i = 0; i < 8; i++)
+                        WRITEL(p_h264->aso_slice_order[i],
+                                S5P_FIMV_E_H264_ASO_SLICE_ORDER_0_V6 + i * 4);
+        }
+        return 0;
+}
+
+/* Encode a single frame */
+int s5p_mfc_encode_one_frame_v6(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        mfc_debug(2, "++\n");
+
+        /* memory structure cur. frame */
+
+        if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+                s5p_mfc_h264_set_aso_slice_order_v6(ctx);
+
+        s5p_mfc_set_slice_mode(ctx);
+
+        WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+        s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+                        S5P_FIMV_CH_FRAME_START_V6, NULL);
+
+        mfc_debug(2, "--\n");
+
+        return 0;
+}
+
+static inline int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
+{
+        unsigned long flags;
+        int new_ctx;
+        int cnt;
+
+        spin_lock_irqsave(&dev->condlock, flags);
+        mfc_debug(2, "Previos context: %d (bits %08lx)\n", dev->curr_ctx,
+                                                        dev->ctx_work_bits);
+        new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
+        cnt = 0;
+        while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
+                new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
+                cnt++;
+                if (cnt > MFC_NUM_CONTEXTS) {
+                        /* No contexts to run */
+                        spin_unlock_irqrestore(&dev->condlock, flags);
+                        return -EAGAIN;
+                }
+        }
+        spin_unlock_irqrestore(&dev->condlock, flags);
+        return new_ctx;
+}
+
+static inline void s5p_mfc_run_dec_last_frames(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf *temp_vb;
+        unsigned long flags;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+
+        /* Frames are being decoded */
+        if (list_empty(&ctx->src_queue)) {
+                mfc_debug(2, "No src buffers.\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return;
+        }
+        /* Get the next source buffer */
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        temp_vb->flags |= MFC_BUF_FLAG_USED;
+        s5p_mfc_set_dec_stream_buffer_v6(ctx,
+                        vb2_dma_contig_plane_dma_addr(temp_vb->b, 0), 0, 0);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_decode_one_frame_v6(ctx, 1);
+}
+
+static inline int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        struct s5p_mfc_buf *temp_vb;
+        unsigned long flags;
+        int last_frame = 0;
+        unsigned int index;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+
+        /* Frames are being decoded */
+        if (list_empty(&ctx->src_queue)) {
+                mfc_debug(2, "No src buffers.\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+        /* Get the next source buffer */
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        temp_vb->flags |= MFC_BUF_FLAG_USED;
+        s5p_mfc_set_dec_stream_buffer_v6(ctx,
+                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
+                        ctx->consumed_stream,
+                        temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+
+        index = temp_vb->b->v4l2_buf.index;
+
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        if (temp_vb->b->v4l2_planes[0].bytesused == 0) {
+                last_frame = 1;
+                mfc_debug(2, "Setting ctx->state to FINISHING\n");
+                ctx->state = MFCINST_FINISHING;
+        }
+        s5p_mfc_decode_one_frame_v6(ctx, last_frame);
+
+        return 0;
+}
+
+static inline int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *dst_mb;
+        struct s5p_mfc_buf *src_mb;
+        unsigned long src_y_addr, src_c_addr, dst_addr;
+        /*
+        unsigned int src_y_size, src_c_size;
+        */
+        unsigned int dst_size;
+        unsigned int index;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+
+        if (list_empty(&ctx->src_queue)) {
+                mfc_debug(2, "no src buffers.\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+
+        if (list_empty(&ctx->dst_queue)) {
+                mfc_debug(2, "no dst buffers.\n");
+                spin_unlock_irqrestore(&dev->irqlock, flags);
+                return -EAGAIN;
+        }
+
+        src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        src_mb->flags |= MFC_BUF_FLAG_USED;
+        src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 0);
+        src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 1);
+
+        mfc_debug(2, "enc src y addr: 0x%08lx", src_y_addr);
+        mfc_debug(2, "enc src c addr: 0x%08lx", src_c_addr);
+
+        s5p_mfc_set_enc_frame_buffer_v6(ctx, src_y_addr, src_c_addr);
+
+        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+        dst_mb->flags |= MFC_BUF_FLAG_USED;
+        dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
+        dst_size = vb2_plane_size(dst_mb->b, 0);
+
+        s5p_mfc_set_enc_stream_buffer_v6(ctx, dst_addr, dst_size);
+
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+
+        index = src_mb->b->v4l2_buf.index;
+
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_encode_one_frame_v6(ctx);
+
+        return 0;
+}
+
+static inline void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *temp_vb;
+
+        /* Initializing decoding - parsing header */
+        spin_lock_irqsave(&dev->irqlock, flags);
+        mfc_debug(2, "Preparing to init decoding.\n");
+        temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+        mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
+        s5p_mfc_set_dec_stream_buffer_v6(ctx,
+                vb2_dma_contig_plane_dma_addr(temp_vb->b, 0), 0,
+                        temp_vb->b->v4l2_planes[0].bytesused);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_init_decode_v6(ctx);
+}
+
+static inline void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        unsigned long flags;
+        struct s5p_mfc_buf *dst_mb;
+        unsigned long dst_addr;
+        unsigned int dst_size;
+
+        spin_lock_irqsave(&dev->irqlock, flags);
+
+        dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+        dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
+        dst_size = vb2_plane_size(dst_mb->b, 0);
+        s5p_mfc_set_enc_stream_buffer_v6(ctx, dst_addr, dst_size);
+        spin_unlock_irqrestore(&dev->irqlock, flags);
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        s5p_mfc_init_encode_v6(ctx);
+}
+
+static inline int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        int ret;
+        /* Header was parsed now start processing
+         * First set the output frame buffers
+         * s5p_mfc_alloc_dec_buffers(ctx); */
+
+        if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
+                mfc_err("It seems that not all destionation buffers were\n"
+                        "mmaped.MFC requires that all destination are mmaped\n"
+                        "before starting processing.\n");
+                return -EAGAIN;
+        }
+
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        ret = s5p_mfc_set_dec_frame_buffer_v6(ctx);
+        if (ret) {
+                mfc_err("Failed to alloc frame mem.\n");
+                ctx->state = MFCINST_ERROR;
+        }
+        return ret;
+}
+
+static inline int s5p_mfc_run_init_enc_buffers(struct s5p_mfc_ctx *ctx)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        int ret;
+
+        ret = s5p_mfc_alloc_codec_buffers_v6(ctx);
+        if (ret) {
+                mfc_err("Failed to allocate encoding buffers.\n");
+                return -ENOMEM;
+        }
+
+        /* Header was generated now starting processing
+         * First set the reference frame buffers
+         */
+        if (ctx->capture_state != QUEUE_BUFS_REQUESTED) {
+                mfc_err("It seems that destionation buffers were not\n"
+                        "requested.MFC requires that header should be generated\n"
+                        "before allocating codec buffer.\n");
+                return -EAGAIN;
+        }
+
+        dev->curr_ctx = ctx->num;
+        s5p_mfc_clean_ctx_int_flags(ctx);
+        ret = s5p_mfc_set_enc_ref_buffer_v6(ctx);
+        if (ret) {
+                mfc_err("Failed to alloc frame mem.\n");
+                ctx->state = MFCINST_ERROR;
+        }
+        return ret;
+}
+
+/* Try running an operation on hardware */
+void s5p_mfc_try_run_v6(struct s5p_mfc_dev *dev)
+{
+        struct s5p_mfc_ctx *ctx;
+        int new_ctx;
+        unsigned int ret = 0;
+
+        mfc_debug(1, "Try run dev: %p\n", dev);
+
+        /* Check whether hardware is not running */
+        if (test_and_set_bit(0, &dev->hw_lock) != 0) {
+                /* This is perfectly ok, the scheduled ctx should wait */
+                mfc_debug(1, "Couldn't lock HW.\n");
+                return;
+        }
+
+        /* Choose the context to run */
+        new_ctx = s5p_mfc_get_new_ctx(dev);
+        if (new_ctx < 0) {
+                /* No contexts to run */
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0) {
+                        mfc_err("Failed to unlock hardware.\n");
+                        return;
+                }
+
+                mfc_debug(1, "No ctx is scheduled to be run.\n");
+                return;
+        }
+
+        mfc_debug(1, "New context: %d\n", new_ctx);
+        ctx = dev->ctx[new_ctx];
+        mfc_debug(1, "Seting new context to %p\n", ctx);
+        /* Got context to run in ctx */
+        mfc_debug(1, "ctx->dst_queue_cnt=%d ctx->dpb_count=%d ctx->src_queue_cnt=%d\n",
+                ctx->dst_queue_cnt, ctx->dpb_count, ctx->src_queue_cnt);
+        mfc_debug(1, "ctx->state=%d\n", ctx->state);
+        /* Last frame has already been sent to MFC
+         * Now obtaining frames from MFC buffer */
+
+        s5p_mfc_clock_on();
+        if (ctx->type == MFCINST_DECODER) {
+                switch (ctx->state) {
+                case MFCINST_FINISHING:
+                        s5p_mfc_run_dec_last_frames(ctx);
+                        break;
+                case MFCINST_RUNNING:
+                        ret = s5p_mfc_run_dec_frame(ctx);
+                        break;
+                case MFCINST_INIT:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_RETURN_INST:
+                        s5p_mfc_clean_ctx_int_flags(ctx);
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_GOT_INST:
+                        s5p_mfc_run_init_dec(ctx);
+                        break;
+                case MFCINST_HEAD_PARSED:
+                        ret = s5p_mfc_run_init_dec_buffers(ctx);
+                        break;
+                case MFCINST_RES_CHANGE_INIT:
+                        s5p_mfc_run_dec_last_frames(ctx);
+                        break;
+                case MFCINST_RES_CHANGE_FLUSH:
+                        s5p_mfc_run_dec_last_frames(ctx);
+                        break;
+                case MFCINST_RES_CHANGE_END:
+                        mfc_debug(2, "Finished remaining frames after resolution change.\n");
+                        ctx->capture_state = QUEUE_FREE;
+                        mfc_debug(2, "Will re-init the codec`.\n");
+                        s5p_mfc_run_init_dec(ctx);
+                        break;
+                default:
+                        ret = -EAGAIN;
+                }
+        } else if (ctx->type == MFCINST_ENCODER) {
+                switch (ctx->state) {
+                case MFCINST_FINISHING:
+                case MFCINST_RUNNING:
+                        ret = s5p_mfc_run_enc_frame(ctx);
+                        break;
+                case MFCINST_INIT:
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_RETURN_INST:
+                        ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+                                        ctx);
+                        break;
+                case MFCINST_GOT_INST:
+                        s5p_mfc_run_init_enc(ctx);
+                        break;
+                case MFCINST_HEAD_PARSED: /* Only for MFC6.x */
+                        ret = s5p_mfc_run_init_enc_buffers(ctx);
+                        break;
+                default:
+                        ret = -EAGAIN;
+                }
+        } else {
+                mfc_err("invalid context type: %d\n", ctx->type);
+                ret = -EAGAIN;
+        }
+
+        if (ret) {
+                /* Free hardware lock */
+                if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+                        mfc_err("Failed to unlock hardware.\n");
+
+                /* This is in deed imporant, as no operation has been
+                 * scheduled, reduce the clock count as no one will
+                 * ever do this, because no interrupt related to this try_run
+                 * will ever come from hardware. */
+                s5p_mfc_clock_off();
+        }
+}
+
+
+void s5p_mfc_cleanup_queue_v6(struct list_head *lh, struct vb2_queue *vq)
+{
+        struct s5p_mfc_buf *b;
+        int i;
+
+        while (!list_empty(lh)) {
+                b = list_entry(lh->next, struct s5p_mfc_buf, list);
+                for (i = 0; i < b->b->num_planes; i++)
+                        vb2_set_plane_payload(b->b, i, 0);
+                vb2_buffer_done(b->b, VB2_BUF_STATE_ERROR);
+                list_del(&b->list);
+        }
+}
+
+void s5p_mfc_clear_int_flags_v6(struct s5p_mfc_dev *dev)
+{
+        mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD_V6);
+        mfc_write(dev, 0, S5P_FIMV_RISC2HOST_INT_V6);
+}
+
+void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
+                unsigned int ofs)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+
+        s5p_mfc_clock_on();
+        WRITEL(data, ofs);
+        s5p_mfc_clock_off();
+}
+
+unsigned int s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
+{
+        struct s5p_mfc_dev *dev = ctx->dev;
+        int ret;
+
+        s5p_mfc_clock_on();
+        ret = READL(ofs);
+        s5p_mfc_clock_off();
+
+        return ret;
+}
+
+int s5p_mfc_get_dspl_y_adr_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6);
+}
+
+int s5p_mfc_get_dec_y_adr_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6);
+}
+
+int s5p_mfc_get_dspl_status_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DISPLAY_STATUS_V6);
+}
+
+int s5p_mfc_get_decoded_status_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DECODED_STATUS_V6);
+}
+
+int s5p_mfc_get_dec_frame_type_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DECODED_FRAME_TYPE_V6) &
+                S5P_FIMV_DECODE_FRAME_MASK_V6;
+}
+
+int s5p_mfc_get_disp_frame_type_v6(struct s5p_mfc_ctx *ctx)
+{
+        return mfc_read(ctx->dev, S5P_FIMV_D_DISPLAY_FRAME_TYPE_V6) &
+                S5P_FIMV_DECODE_FRAME_MASK_V6;
+}
+
+int s5p_mfc_get_consumed_stream_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DECODED_NAL_SIZE_V6);
+}
+
+int s5p_mfc_get_int_reason_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_RISC2HOST_CMD_V6) &
+                S5P_FIMV_RISC2HOST_CMD_MASK;
+}
+
+int s5p_mfc_get_int_err_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_ERROR_CODE_V6);
+}
+
+int s5p_mfc_err_dec_v6(unsigned int err)
+{
+        return (err & S5P_FIMV_ERR_DEC_MASK_V6) >> S5P_FIMV_ERR_DEC_SHIFT_V6;
+}
+
+int s5p_mfc_err_dspl_v6(unsigned int err)
+{
+        return (err & S5P_FIMV_ERR_DSPL_MASK_V6) >> S5P_FIMV_ERR_DSPL_SHIFT_V6;
+}
+
+int s5p_mfc_get_img_width_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V6);
+}
+
+int s5p_mfc_get_img_height_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V6);
+}
+
+int s5p_mfc_get_dpb_count_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_MIN_NUM_DPB_V6);
+}
+
+int s5p_mfc_get_mv_count_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_MIN_NUM_MV_V6);
+}
+
+int s5p_mfc_get_inst_no_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_RET_INSTANCE_ID_V6);
+}
+
+int s5p_mfc_get_enc_dpb_count_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_E_NUM_DPB_V6);
+}
+
+int s5p_mfc_get_enc_strm_size_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_E_STREAM_SIZE_V6);
+}
+
+int s5p_mfc_get_enc_slice_type_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_E_SLICE_TYPE_V6);
+}
+
+int s5p_mfc_get_enc_pic_count_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_E_PICTURE_COUNT_V6);
+}
+
+int s5p_mfc_get_sei_avail_status_v6(struct s5p_mfc_ctx *ctx)
+{
+        return mfc_read(ctx->dev, S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V6);
+}
+
+int s5p_mfc_get_mvc_num_views_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_MVC_NUM_VIEWS_V6);
+}
+
+int s5p_mfc_get_mvc_view_id_v6(struct s5p_mfc_dev *dev)
+{
+        return mfc_read(dev, S5P_FIMV_D_MVC_VIEW_ID_V6);
+}
+
+unsigned int s5p_mfc_get_pic_type_top_v6(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v6(ctx, PIC_TIME_TOP_V6);
+}
+
+unsigned int s5p_mfc_get_pic_type_bot_v6(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v6(ctx, PIC_TIME_BOT_V6);
+}
+
+unsigned int s5p_mfc_get_crop_info_h_v6(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v6(ctx, CROP_INFO_H_V6);
+}
+
+unsigned int s5p_mfc_get_crop_info_v_v6(struct s5p_mfc_ctx *ctx)
+{
+        return s5p_mfc_read_info_v6(ctx, CROP_INFO_V_V6);
+}
+
+/* Initialize opr function pointers for MFC v6 */
+static struct s5p_mfc_hw_ops s5p_mfc_ops_v6 = {
+        .alloc_dec_temp_buffers = s5p_mfc_alloc_dec_temp_buffers_v6,
+        .release_dec_desc_buffer = s5p_mfc_release_dec_desc_buffer_v6,
+        .alloc_codec_buffers = s5p_mfc_alloc_codec_buffers_v6,
+        .release_codec_buffers = s5p_mfc_release_codec_buffers_v6,
+        .alloc_instance_buffer = s5p_mfc_alloc_instance_buffer_v6,
+        .release_instance_buffer = s5p_mfc_release_instance_buffer_v6,
+        .alloc_dev_context_buffer =
+                s5p_mfc_alloc_dev_context_buffer_v6,
+        .release_dev_context_buffer =
+                s5p_mfc_release_dev_context_buffer_v6,
+        .dec_calc_dpb_size = s5p_mfc_dec_calc_dpb_size_v6,
+        .enc_calc_src_size = s5p_mfc_enc_calc_src_size_v6,
+        .set_dec_stream_buffer = s5p_mfc_set_dec_stream_buffer_v6,
+        .set_dec_frame_buffer = s5p_mfc_set_dec_frame_buffer_v6,
+        .set_enc_stream_buffer = s5p_mfc_set_enc_stream_buffer_v6,
+        .set_enc_frame_buffer = s5p_mfc_set_enc_frame_buffer_v6,
+        .get_enc_frame_buffer = s5p_mfc_get_enc_frame_buffer_v6,
+        .set_enc_ref_buffer = s5p_mfc_set_enc_ref_buffer_v6,
+        .init_decode = s5p_mfc_init_decode_v6,
+        .init_encode = s5p_mfc_init_encode_v6,
+        .encode_one_frame = s5p_mfc_encode_one_frame_v6,
+        .try_run = s5p_mfc_try_run_v6,
+        .cleanup_queue = s5p_mfc_cleanup_queue_v6,
+        .clear_int_flags = s5p_mfc_clear_int_flags_v6,
+        .write_info = s5p_mfc_write_info_v6,
+        .read_info = s5p_mfc_read_info_v6,
+        .get_dspl_y_adr = s5p_mfc_get_dspl_y_adr_v6,
+        .get_dec_y_adr = s5p_mfc_get_dec_y_adr_v6,
+        .get_dspl_status = s5p_mfc_get_dspl_status_v6,
+        .get_dec_status = s5p_mfc_get_dec_status_v6,
+        .get_dec_frame_type = s5p_mfc_get_dec_frame_type_v6,
+        .get_disp_frame_type = s5p_mfc_get_disp_frame_type_v6,
+        .get_consumed_stream = s5p_mfc_get_consumed_stream_v6,
+        .get_int_reason = s5p_mfc_get_int_reason_v6,
+        .get_int_err = s5p_mfc_get_int_err_v6,
+        .err_dec = s5p_mfc_err_dec_v6,
+        .err_dspl = s5p_mfc_err_dspl_v6,
+        .get_img_width = s5p_mfc_get_img_width_v6,
+        .get_img_height = s5p_mfc_get_img_height_v6,
+        .get_dpb_count = s5p_mfc_get_dpb_count_v6,
+        .get_mv_count = s5p_mfc_get_mv_count_v6,
+        .get_inst_no = s5p_mfc_get_inst_no_v6,
+        .get_enc_strm_size = s5p_mfc_get_enc_strm_size_v6,
+        .get_enc_slice_type = s5p_mfc_get_enc_slice_type_v6,
+        .get_enc_dpb_count = s5p_mfc_get_enc_dpb_count_v6,
+        .get_enc_pic_count = s5p_mfc_get_enc_pic_count_v6,
+        .get_sei_avail_status = s5p_mfc_get_sei_avail_status_v6,
+        .get_mvc_num_views = s5p_mfc_get_mvc_num_views_v6,
+        .get_mvc_view_id = s5p_mfc_get_mvc_view_id_v6,
+        .get_pic_type_top = s5p_mfc_get_pic_type_top_v6,
+        .get_pic_type_bot = s5p_mfc_get_pic_type_bot_v6,
+        .get_crop_info_h = s5p_mfc_get_crop_info_h_v6,
+        .get_crop_info_v = s5p_mfc_get_crop_info_v_v6,
+};
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v6(void)
+{
+        return &s5p_mfc_ops_v6;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
new file mode 100644
index 000000000000..ab164efa127e
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
@@ -0,0 +1,50 @@
+/*
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
+ *
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * Contains declarations of hw related functions.
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef S5P_MFC_OPR_V6_H_
+#define S5P_MFC_OPR_V6_H_
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_opr.h"
+
+#define MFC_CTRL_MODE_CUSTOM    MFC_CTRL_MODE_SFR
+
+#define MB_WIDTH(x_size)                DIV_ROUND_UP(x_size, 16)
+#define MB_HEIGHT(y_size)               DIV_ROUND_UP(y_size, 16)
+#define S5P_MFC_DEC_MV_SIZE_V6(x, y)    (MB_WIDTH(x) * \
+                                        (((MB_HEIGHT(y)+1)/2)*2) * 64 + 128)
+
+/* Definition */
+#define ENC_MULTI_SLICE_MB_MAX          ((1 << 30) - 1)
+#define ENC_MULTI_SLICE_BIT_MIN         2800
+#define ENC_INTRA_REFRESH_MB_MAX        ((1 << 18) - 1)
+#define ENC_VBV_BUF_SIZE_MAX            ((1 << 30) - 1)
+#define ENC_H264_LOOP_FILTER_AB_MIN     -12
+#define ENC_H264_LOOP_FILTER_AB_MAX     12
+#define ENC_H264_RC_FRAME_RATE_MAX      ((1 << 16) - 1)
+#define ENC_H263_RC_FRAME_RATE_MAX      ((1 << 16) - 1)
+#define ENC_H264_PROFILE_MAX            3
+#define ENC_H264_LEVEL_MAX              42
+#define ENC_MPEG4_VOP_TIME_RES_MAX      ((1 << 16) - 1)
+#define FRAME_DELTA_H264_H263           1
+#define TIGHT_CBR_MAX                   10
+
+/* Definitions for shared memory compatibility */
+#define PIC_TIME_TOP_V6         S5P_FIMV_D_RET_PICTURE_TAG_TOP_V6
+#define PIC_TIME_BOT_V6         S5P_FIMV_D_RET_PICTURE_TAG_BOT_V6
+#define CROP_INFO_H_V6          S5P_FIMV_D_DISPLAY_CROP_INFO1_V6
+#define CROP_INFO_V_V6          S5P_FIMV_D_DISPLAY_CROP_INFO2_V6
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v6(void);
+#endif /* S5P_MFC_OPR_V6_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c b/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
index 0503d14ac94e..367db7552289 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
@@ -20,7 +20,6 @@
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_pm.h"
 
-#define MFC_CLKNAME             "sclk_mfc"
 #define MFC_GATE_CLK_NAME       "mfc"
 
 #define CLK_DEBUG
@@ -51,7 +50,7 @@ int s5p_mfc_init_pm(struct s5p_mfc_dev *dev)
                 goto err_p_ip_clk;
         }
 
-        pm->clock = clk_get(&dev->plat_dev->dev, MFC_CLKNAME);
+        pm->clock = clk_get(&dev->plat_dev->dev, dev->variant->mclk_name);
         if (IS_ERR(pm->clock)) {
                 mfc_err("Failed to get MFC clock\n");
                 ret = PTR_ERR(pm->clock);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_shm.c b/drivers/media/platform/s5p-mfc/s5p_mfc_shm.c
deleted file mode 100644
index b5933d233a4b..000000000000
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_shm.c
+++ /dev/null
@@ -1,47 +0,0 @@
-/*
- * linux/drivers/media/platform/s5p-mfc/s5p_mfc_shm.c
- *
- * Copyright (c) 2010 Samsung Electronics Co., Ltd.
- *              http://www.samsung.com/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#ifdef CONFIG_ARCH_EXYNOS4
-#include <linux/dma-mapping.h>
-#endif
-#include <linux/io.h>
-#include "s5p_mfc_common.h"
-#include "s5p_mfc_debug.h"
-
-int s5p_mfc_init_shm(struct s5p_mfc_ctx *ctx)
-{
-        struct s5p_mfc_dev *dev = ctx->dev;
-        void *shm_alloc_ctx = dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
-
-        ctx->shm_alloc = vb2_dma_contig_memops.alloc(shm_alloc_ctx,
-                                                        SHARED_BUF_SIZE);
-        if (IS_ERR(ctx->shm_alloc)) {
-                mfc_err("failed to allocate shared memory\n");
-                return PTR_ERR(ctx->shm_alloc);
-        }
-        /* shm_ofs only keeps the offset from base (port a) */
-        ctx->shm_ofs = s5p_mfc_mem_cookie(shm_alloc_ctx, ctx->shm_alloc)
-                                                                - dev->bank1;
-        BUG_ON(ctx->shm_ofs & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
-        ctx->shm = vb2_dma_contig_memops.vaddr(ctx->shm_alloc);
-        if (!ctx->shm) {
-                vb2_dma_contig_memops.put(ctx->shm_alloc);
-                ctx->shm_ofs = 0;
-                ctx->shm_alloc = NULL;
-                mfc_err("failed to virt addr of shared memory\n");
-                return -ENOMEM;
-        }
-        memset((void *)ctx->shm, 0, SHARED_BUF_SIZE);
-        wmb();
-        return 0;
-}
-
diff --git a/drivers/media/platform/soc_camera/mx2_camera.c b/drivers/media/platform/soc_camera/mx2_camera.c
index 403d7f17bfab..9fd9d1c5b218 100644
--- a/drivers/media/platform/soc_camera/mx2_camera.c
+++ b/drivers/media/platform/soc_camera/mx2_camera.c
@@ -1376,6 +1376,7 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
         __u32 pixfmt = pix->pixelformat;
         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
         struct mx2_camera_dev *pcdev = ici->priv;
+        struct mx2_fmt_cfg *emma_prp;
         unsigned int width_limit;
         int ret;
 
@@ -1438,12 +1439,11 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
                 __func__, pcdev->s_width, pcdev->s_height);
 
         /* If the sensor does not support image size try PrP resizing */
-        pcdev->emma_prp = mx27_emma_prp_get_format(xlate->code,
+        emma_prp = mx27_emma_prp_get_format(xlate->code,
                                                    xlate->host_fmt->fourcc);
 
-        memset(pcdev->resizing, 0, sizeof(pcdev->resizing));
         if ((mf.width != pix->width || mf.height != pix->height) &&
-                pcdev->emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) {
+                emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) {
                 if (mx2_emmaprp_resize(pcdev, &mf, pix, false) < 0)
                         dev_dbg(icd->parent, "%s: can't resize\n", __func__);
         }
@@ -1655,6 +1655,7 @@ static int __devinit mx27_camera_emma_init(struct platform_device *pdev)
         irq_emma = platform_get_irq(pdev, 1);
         if (!res_emma || !irq_emma) {
                 dev_err(pcdev->dev, "no EMMA resources\n");
+                err = -ENODEV;
                 goto out;
         }
 
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index 3be92944f8e7..d3f0b84e2d70 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -950,11 +950,11 @@ static int soc_camera_s_selection(struct file *file, void *fh,
 
         /* In all these cases cropping emulation will not help */
         if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-            (s->target != V4L2_SEL_TGT_COMPOSE_ACTIVE &&
-             s->target != V4L2_SEL_TGT_CROP_ACTIVE))
+            (s->target != V4L2_SEL_TGT_COMPOSE &&
+             s->target != V4L2_SEL_TGT_CROP))
                 return -EINVAL;
 
-        if (s->target == V4L2_SEL_TGT_COMPOSE_ACTIVE) {
+        if (s->target == V4L2_SEL_TGT_COMPOSE) {
                 /* No output size change during a running capture! */
                 if (is_streaming(ici, icd) &&
                     (icd->user_width != s->r.width ||
@@ -974,7 +974,7 @@ static int soc_camera_s_selection(struct file *file, void *fh,
 
         ret = ici->ops->set_selection(icd, s);
         if (!ret &&
-            s->target == V4L2_SEL_TGT_COMPOSE_ACTIVE) {
+            s->target == V4L2_SEL_TGT_COMPOSE) {
                 icd->user_width = s->r.width;
                 icd->user_height = s->r.height;
                 if (!icd->streamer)
@@ -1184,7 +1184,8 @@ static int soc_camera_probe(struct soc_camera_device *icd)
         sd->grp_id = soc_camera_grp_id(icd);
         v4l2_set_subdev_hostdata(sd, icd);
 
-        if (v4l2_ctrl_add_handler(&icd->ctrl_handler, sd->ctrl_handler, NULL))
+        ret = v4l2_ctrl_add_handler(&icd->ctrl_handler, sd->ctrl_handler, NULL);
+        if (ret < 0)
                 goto ectrl;
 
         /* At this point client .probe() should have run already */
@@ -1529,12 +1530,11 @@ static int __devinit soc_camera_pdrv_probe(struct platform_device *pdev)
 {
         struct soc_camera_link *icl = pdev->dev.platform_data;
         struct soc_camera_device *icd;
-        int ret;
 
         if (!icl)
                 return -EINVAL;
 
-        icd = kzalloc(sizeof(*icd), GFP_KERNEL);
+        icd = devm_kzalloc(&pdev->dev, sizeof(*icd), GFP_KERNEL);
         if (!icd)
                 return -ENOMEM;
 
@@ -1543,19 +1543,10 @@ static int __devinit soc_camera_pdrv_probe(struct platform_device *pdev)
         icd->pdev = &pdev->dev;
         platform_set_drvdata(pdev, icd);
 
-        ret = soc_camera_device_register(icd);
-        if (ret < 0)
-                goto escdevreg;
-
         icd->user_width         = DEFAULT_WIDTH;
         icd->user_height        = DEFAULT_HEIGHT;
 
-        return 0;
-
-escdevreg:
-        kfree(icd);
-
-        return ret;
+        return soc_camera_device_register(icd);
 }
 
 /*
@@ -1572,8 +1563,6 @@ static int __devexit soc_camera_pdrv_remove(struct platform_device *pdev)
 
         list_del(&icd->list);
 
-        kfree(icd);
-
         return 0;
 }
 
@@ -1586,18 +1575,7 @@ static struct platform_driver __refdata soc_camera_pdrv = {
         },
 };
 
-static int __init soc_camera_init(void)
-{
-        return platform_driver_register(&soc_camera_pdrv);
-}
-
-static void __exit soc_camera_exit(void)
-{
-        platform_driver_unregister(&soc_camera_pdrv);
-}
-
-module_init(soc_camera_init);
-module_exit(soc_camera_exit);
+module_platform_driver(soc_camera_pdrv);
 
 MODULE_DESCRIPTION("Image capture bus driver");
 MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
diff --git a/drivers/media/radio/si470x/radio-si470x-i2c.c b/drivers/media/radio/si470x/radio-si470x-i2c.c
index e5024cfd27a7..4ef55ec8045e 100644
--- a/drivers/media/radio/si470x/radio-si470x-i2c.c
+++ b/drivers/media/radio/si470x/radio-si470x-i2c.c
@@ -308,7 +308,7 @@ static irqreturn_t si470x_i2c_interrupt(int irq, void *dev_id)
                                         READCHAN_BLERD) >> 10;
                         rds = radio->registers[RDSD];
                         break;
-                };
+                }
 
                 /* Fill the V4L2 RDS buffer */
                 put_unaligned_le16(rds, &tmpbuf);
diff --git a/drivers/media/radio/si470x/radio-si470x-usb.c b/drivers/media/radio/si470x/radio-si470x-usb.c
index be076f7181e7..62f3edec39bc 100644
--- a/drivers/media/radio/si470x/radio-si470x-usb.c
+++ b/drivers/media/radio/si470x/radio-si470x-usb.c
@@ -446,7 +446,7 @@ static void si470x_int_in_callback(struct urb *urb)
                                                 READCHAN_BLERD) >> 10;
                                 rds = radio->registers[RDSD];
                                 break;
-                        };
+                        }
 
                         /* Fill the V4L2 RDS buffer */
                         put_unaligned_le16(rds, &tmpbuf);
diff --git a/drivers/media/radio/si4713-i2c.c b/drivers/media/radio/si4713-i2c.c
index a9e6d17015ef..e3079c142c5f 100644
--- a/drivers/media/radio/si4713-i2c.c
+++ b/drivers/media/radio/si4713-i2c.c
@@ -1009,7 +1009,7 @@ static int si4713_choose_econtrol_action(struct si4713_device *sdev, u32 id,
 
         default:
                 rval = -EINVAL;
-        };
+        }
 
         return rval;
 }
@@ -1081,7 +1081,7 @@ static int si4713_write_econtrol_string(struct si4713_device *sdev,
         default:
                 rval = -EINVAL;
                 break;
-        };
+        }
 
 exit:
         return rval;
@@ -1130,7 +1130,7 @@ static int si4713_write_econtrol_tune(struct si4713_device *sdev,
         default:
                 rval = -EINVAL;
                 goto unlock;
-        };
+        }
 
         if (sdev->power_state)
                 rval = si4713_tx_tune_power(sdev, power, antcap);
@@ -1420,7 +1420,7 @@ static int si4713_read_econtrol_string(struct si4713_device *sdev,
         default:
                 rval = -EINVAL;
                 break;
-        };
+        }
 
 exit:
         return rval;
@@ -1473,7 +1473,7 @@ static int si4713_read_econtrol_tune(struct si4713_device *sdev,
                 break;
         default:
                 rval = -EINVAL;
-        };
+        }
 
 unlock:
         mutex_unlock(&sdev->mutex);
@@ -1698,7 +1698,7 @@ static int si4713_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
         default:
                 rval = -EINVAL;
                 break;
-        };
+        }
 
         return rval;
 }
diff --git a/drivers/media/rc/ene_ir.c b/drivers/media/rc/ene_ir.c
index 647dd951b0e8..d05ac15b5de4 100644
--- a/drivers/media/rc/ene_ir.c
+++ b/drivers/media/rc/ene_ir.c
@@ -881,10 +881,13 @@ static int ene_set_tx_mask(struct rc_dev *rdev, u32 tx_mask)
 static int ene_set_tx_carrier(struct rc_dev *rdev, u32 carrier)
 {
         struct ene_device *dev = rdev->priv;
-        u32 period = 2000000 / carrier;
+        u32 period;
 
         dbg("TX: attempt to set tx carrier to %d kHz", carrier);
+        if (carrier == 0)
+                return -EINVAL;
 
+        period = 2000000 / carrier;
         if (period && (period > ENE_CIRMOD_PRD_MAX ||
                         period < ENE_CIRMOD_PRD_MIN)) {
 
diff --git a/drivers/media/rc/iguanair.c b/drivers/media/rc/iguanair.c
index 1e4c68a5cecf..51d7057aca04 100644
--- a/drivers/media/rc/iguanair.c
+++ b/drivers/media/rc/iguanair.c
@@ -28,6 +28,7 @@
 #include <media/rc-core.h>
 
 #define DRIVER_NAME "iguanair"
+#define BUF_SIZE 152
 
 struct iguanair {
         struct rc_dev *rc;
@@ -35,26 +36,23 @@ struct iguanair {
         struct device *dev;
         struct usb_device *udev;
 
-        int pipe_out;
         uint16_t version;
         uint8_t bufsize;
+        uint8_t cycle_overhead;
 
         struct mutex lock;
 
         /* receiver support */
         bool receiver_on;
-        dma_addr_t dma_in;
+        dma_addr_t dma_in, dma_out;
         uint8_t *buf_in;
-        struct urb *urb_in;
+        struct urb *urb_in, *urb_out;
         struct completion completion;
 
         /* transmit support */
         bool tx_overflow;
         uint32_t carrier;
-        uint8_t cycle_overhead;
-        uint8_t channels;
-        uint8_t busy4;
-        uint8_t busy7;
+        struct send_packet *packet;
 
         char name[64];
         char phys[64];
@@ -73,7 +71,8 @@ struct iguanair {
 #define DIR_IN                  0xdc
 #define DIR_OUT                 0xcd
 
-#define MAX_PACKET_SIZE         8u
+#define MAX_IN_PACKET           8u
+#define MAX_OUT_PACKET          (sizeof(struct send_packet) + BUF_SIZE)
 #define TIMEOUT                 1000
 #define RX_RESOLUTION           21333
 
@@ -191,20 +190,25 @@ static void iguanair_rx(struct urb *urb)
                 dev_warn(ir->dev, "failed to resubmit urb: %d\n", rc);
 }
 
-static int iguanair_send(struct iguanair *ir, void *data, unsigned size)
+static void iguanair_irq_out(struct urb *urb)
 {
-        int rc, transferred;
+        struct iguanair *ir = urb->context;
+
+        if (urb->status)
+                dev_dbg(ir->dev, "Error: out urb status = %d\n", urb->status);
+}
+
+static int iguanair_send(struct iguanair *ir, unsigned size)
+{
+        int rc;
 
         INIT_COMPLETION(ir->completion);
 
-        rc = usb_interrupt_msg(ir->udev, ir->pipe_out, data, size,
-                                                        &transferred, TIMEOUT);
+        ir->urb_out->transfer_buffer_length = size;
+        rc = usb_submit_urb(ir->urb_out, GFP_KERNEL);
         if (rc)
                 return rc;
 
-        if (transferred != size)
-                return -EIO;
-
         if (wait_for_completion_timeout(&ir->completion, TIMEOUT) == 0)
                 return -ETIMEDOUT;
 
@@ -213,14 +217,13 @@ static int iguanair_send(struct iguanair *ir, void *data, unsigned size)
 
 static int iguanair_get_features(struct iguanair *ir)
 {
-        struct packet packet;
         int rc;
 
-        packet.start = 0;
-        packet.direction = DIR_OUT;
-        packet.cmd = CMD_GET_VERSION;
+        ir->packet->header.start = 0;
+        ir->packet->header.direction = DIR_OUT;
+        ir->packet->header.cmd = CMD_GET_VERSION;
 
-        rc = iguanair_send(ir, &packet, sizeof(packet));
+        rc = iguanair_send(ir, sizeof(ir->packet->header));
         if (rc) {
                 dev_info(ir->dev, "failed to get version\n");
                 goto out;
@@ -235,17 +238,23 @@ static int iguanair_get_features(struct iguanair *ir)
         ir->bufsize = 150;
         ir->cycle_overhead = 65;
 
-        packet.cmd = CMD_GET_BUFSIZE;
+        ir->packet->header.cmd = CMD_GET_BUFSIZE;
 
-        rc = iguanair_send(ir, &packet, sizeof(packet));
+        rc = iguanair_send(ir, sizeof(ir->packet->header));
         if (rc) {
                 dev_info(ir->dev, "failed to get buffer size\n");
                 goto out;
         }
 
-        packet.cmd = CMD_GET_FEATURES;
+        if (ir->bufsize > BUF_SIZE) {
+                dev_info(ir->dev, "buffer size %u larger than expected\n",
+                                                                ir->bufsize);
+                ir->bufsize = BUF_SIZE;
+        }
+
+        ir->packet->header.cmd = CMD_GET_FEATURES;
 
-        rc = iguanair_send(ir, &packet, sizeof(packet));
+        rc = iguanair_send(ir, sizeof(ir->packet->header));
         if (rc) {
                 dev_info(ir->dev, "failed to get features\n");
                 goto out;
@@ -257,13 +266,18 @@ out:
 
 static int iguanair_receiver(struct iguanair *ir, bool enable)
 {
-        struct packet packet = { 0, DIR_OUT, enable ?
-                                CMD_RECEIVER_ON : CMD_RECEIVER_OFF };
+        int rc;
+
+        ir->packet->header.start = 0;
+        ir->packet->header.direction = DIR_OUT;
+        ir->packet->header.cmd = enable ? CMD_RECEIVER_ON : CMD_RECEIVER_OFF;
 
         if (enable)
                 ir_raw_event_reset(ir->rc);
 
-        return iguanair_send(ir, &packet, sizeof(packet));
+        rc = iguanair_send(ir, sizeof(ir->packet->header));
+
+        return rc;
 }
 
 /*
@@ -308,8 +322,8 @@ static int iguanair_set_tx_carrier(struct rc_dev *dev, uint32_t carrier)
                 fours = (cycles - sevens * 7) / 4;
 
                 /* magic happens here */
-                ir->busy7 = (4 - sevens) * 2;
-                ir->busy4 = 110 - fours;
+                ir->packet->busy7 = (4 - sevens) * 2;
+                ir->packet->busy4 = 110 - fours;
         }
 
         mutex_unlock(&ir->lock);
@@ -325,7 +339,7 @@ static int iguanair_set_tx_mask(struct rc_dev *dev, uint32_t mask)
                 return 4;
 
         mutex_lock(&ir->lock);
-        ir->channels = mask;
+        ir->packet->channels = mask << 4;
         mutex_unlock(&ir->lock);
 
         return 0;
@@ -337,16 +351,9 @@ static int iguanair_tx(struct rc_dev *dev, unsigned *txbuf, unsigned count)
         uint8_t space;
         unsigned i, size, periods, bytes;
         int rc;
-        struct send_packet *packet;
 
         mutex_lock(&ir->lock);
 
-        packet = kmalloc(sizeof(*packet) + ir->bufsize, GFP_KERNEL);
-        if (!packet) {
-                rc = -ENOMEM;
-                goto out;
-        }
-
         /* convert from us to carrier periods */
         for (i = space = size = 0; i < count; i++) {
                 periods = DIV_ROUND_CLOSEST(txbuf[i] * ir->carrier, 1000000);
@@ -356,11 +363,11 @@ static int iguanair_tx(struct rc_dev *dev, unsigned *txbuf, unsigned count)
                         break;
                 }
                 while (periods > 127) {
-                        packet->payload[size++] = 127 | space;
+                        ir->packet->payload[size++] = 127 | space;
                         periods -= 127;
                 }
 
-                packet->payload[size++] = periods | space;
+                ir->packet->payload[size++] = periods | space;
                 space ^= 0x80;
         }
 
@@ -369,36 +376,19 @@ static int iguanair_tx(struct rc_dev *dev, unsigned *txbuf, unsigned count)
                 goto out;
         }
 
-        packet->header.start = 0;
-        packet->header.direction = DIR_OUT;
-        packet->header.cmd = CMD_SEND;
-        packet->length = size;
-        packet->channels = ir->channels << 4;
-        packet->busy7 = ir->busy7;
-        packet->busy4 = ir->busy4;
-
-        if (ir->receiver_on) {
-                rc = iguanair_receiver(ir, false);
-                if (rc) {
-                        dev_warn(ir->dev, "disable receiver before transmit failed\n");
-                        goto out;
-                }
-        }
+        ir->packet->header.start = 0;
+        ir->packet->header.direction = DIR_OUT;
+        ir->packet->header.cmd = CMD_SEND;
+        ir->packet->length = size;
 
         ir->tx_overflow = false;
 
-        rc = iguanair_send(ir, packet, size + 8);
+        rc = iguanair_send(ir, sizeof(*ir->packet) + size);
 
         if (rc == 0 && ir->tx_overflow)
                 rc = -EOVERFLOW;
 
-        if (ir->receiver_on) {
-                if (iguanair_receiver(ir, true))
-                        dev_warn(ir->dev, "re-enable receiver after transmit failed\n");
-        }
-
 out:
-        kfree(packet);
         mutex_unlock(&ir->lock);
 
         return rc ? rc : count;
@@ -411,8 +401,6 @@ static int iguanair_open(struct rc_dev *rdev)
 
         mutex_lock(&ir->lock);
 
-        BUG_ON(ir->receiver_on);
-
         rc = iguanair_receiver(ir, true);
         if (rc == 0)
                 ir->receiver_on = true;
@@ -443,7 +431,7 @@ static int __devinit iguanair_probe(struct usb_interface *intf,
         struct usb_device *udev = interface_to_usbdev(intf);
         struct iguanair *ir;
         struct rc_dev *rc;
-        int ret, pipein;
+        int ret, pipein, pipeout;
         struct usb_host_interface *idesc;
 
         ir = kzalloc(sizeof(*ir), GFP_KERNEL);
@@ -453,11 +441,14 @@ static int __devinit iguanair_probe(struct usb_interface *intf,
                 goto out;
         }
 
-        ir->buf_in = usb_alloc_coherent(udev, MAX_PACKET_SIZE, GFP_KERNEL,
+        ir->buf_in = usb_alloc_coherent(udev, MAX_IN_PACKET, GFP_KERNEL,
                                                                 &ir->dma_in);
+        ir->packet = usb_alloc_coherent(udev, MAX_OUT_PACKET, GFP_KERNEL,
+                                                                &ir->dma_out);
         ir->urb_in = usb_alloc_urb(0, GFP_KERNEL);
+        ir->urb_out = usb_alloc_urb(0, GFP_KERNEL);
 
-        if (!ir->buf_in || !ir->urb_in) {
+        if (!ir->buf_in || !ir->packet || !ir->urb_in || !ir->urb_out) {
                 ret = -ENOMEM;
                 goto out;
         }
@@ -472,13 +463,18 @@ static int __devinit iguanair_probe(struct usb_interface *intf,
         ir->rc = rc;
         ir->dev = &intf->dev;
         ir->udev = udev;
-        ir->pipe_out = usb_sndintpipe(udev,
-                                idesc->endpoint[1].desc.bEndpointAddress);
         mutex_init(&ir->lock);
+
         init_completion(&ir->completion);
+        pipeout = usb_sndintpipe(udev,
+                                idesc->endpoint[1].desc.bEndpointAddress);
+        usb_fill_int_urb(ir->urb_out, udev, pipeout, ir->packet, MAX_OUT_PACKET,
+                                                iguanair_irq_out, ir, 1);
+        ir->urb_out->transfer_dma = ir->dma_out;
+        ir->urb_out->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 
         pipein = usb_rcvintpipe(udev, idesc->endpoint[0].desc.bEndpointAddress);
-        usb_fill_int_urb(ir->urb_in, udev, pipein, ir->buf_in, MAX_PACKET_SIZE,
+        usb_fill_int_urb(ir->urb_in, udev, pipein, ir->buf_in, MAX_IN_PACKET,
                                                          iguanair_rx, ir, 1);
         ir->urb_in->transfer_dma = ir->dma_in;
         ir->urb_in->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
@@ -528,11 +524,14 @@ static int __devinit iguanair_probe(struct usb_interface *intf,
         return 0;
 out2:
         usb_kill_urb(ir->urb_in);
+        usb_kill_urb(ir->urb_out);
 out:
         if (ir) {
                 usb_free_urb(ir->urb_in);
-                usb_free_coherent(udev, MAX_PACKET_SIZE, ir->buf_in,
-                                                                ir->dma_in);
+                usb_free_urb(ir->urb_out);
+                usb_free_coherent(udev, MAX_IN_PACKET, ir->buf_in, ir->dma_in);
+                usb_free_coherent(udev, MAX_OUT_PACKET, ir->packet,
+                                                                ir->dma_out);
         }
         rc_free_device(rc);
         kfree(ir);
@@ -546,8 +545,11 @@ static void __devexit iguanair_disconnect(struct usb_interface *intf)
         rc_unregister_device(ir->rc);
         usb_set_intfdata(intf, NULL);
         usb_kill_urb(ir->urb_in);
+        usb_kill_urb(ir->urb_out);
         usb_free_urb(ir->urb_in);
-        usb_free_coherent(ir->udev, MAX_PACKET_SIZE, ir->buf_in, ir->dma_in);
+        usb_free_urb(ir->urb_out);
+        usb_free_coherent(ir->udev, MAX_IN_PACKET, ir->buf_in, ir->dma_in);
+        usb_free_coherent(ir->udev, MAX_OUT_PACKET, ir->packet, ir->dma_out);
         kfree(ir);
 }
 
@@ -565,6 +567,7 @@ static int iguanair_suspend(struct usb_interface *intf, pm_message_t message)
         }
 
         usb_kill_urb(ir->urb_in);
+        usb_kill_urb(ir->urb_out);
 
         mutex_unlock(&ir->lock);
 
diff --git a/drivers/media/rc/ir-lirc-codec.c b/drivers/media/rc/ir-lirc-codec.c
index 569124b03de3..870c93052fd0 100644
--- a/drivers/media/rc/ir-lirc-codec.c
+++ b/drivers/media/rc/ir-lirc-codec.c
@@ -203,13 +203,13 @@ static long ir_lirc_ioctl(struct file *filep, unsigned int cmd,
         /* TX settings */
         case LIRC_SET_TRANSMITTER_MASK:
                 if (!dev->s_tx_mask)
-                        return -EINVAL;
+                        return -ENOSYS;
 
                 return dev->s_tx_mask(dev, val);
 
         case LIRC_SET_SEND_CARRIER:
                 if (!dev->s_tx_carrier)
-                        return -EINVAL;
+                        return -ENOSYS;
 
                 return dev->s_tx_carrier(dev, val);
 
diff --git a/drivers/media/rc/keymaps/rc-msi-digivox-ii.c b/drivers/media/rc/keymaps/rc-msi-digivox-ii.c
index c64e9e30045d..2fa71d0d72d7 100644
--- a/drivers/media/rc/keymaps/rc-msi-digivox-ii.c
+++ b/drivers/media/rc/keymaps/rc-msi-digivox-ii.c
@@ -22,24 +22,24 @@
 #include <linux/module.h>
 
 static struct rc_map_table msi_digivox_ii[] = {
-        { 0x0002, KEY_2 },
-        { 0x0003, KEY_UP },              /* up */
-        { 0x0004, KEY_3 },
-        { 0x0005, KEY_CHANNELDOWN },
-        { 0x0008, KEY_5 },
-        { 0x0009, KEY_0 },
-        { 0x000b, KEY_8 },
-        { 0x000d, KEY_DOWN },            /* down */
-        { 0x0010, KEY_9 },
-        { 0x0011, KEY_7 },
-        { 0x0014, KEY_VOLUMEUP },
-        { 0x0015, KEY_CHANNELUP },
-        { 0x0016, KEY_OK },
-        { 0x0017, KEY_POWER2 },
-        { 0x001a, KEY_1 },
-        { 0x001c, KEY_4 },
-        { 0x001d, KEY_6 },
-        { 0x001f, KEY_VOLUMEDOWN },
+        { 0x0302, KEY_2 },
+        { 0x0303, KEY_UP },              /* up */
+        { 0x0304, KEY_3 },
+        { 0x0305, KEY_CHANNELDOWN },
+        { 0x0308, KEY_5 },
+        { 0x0309, KEY_0 },
+        { 0x030b, KEY_8 },
+        { 0x030d, KEY_DOWN },            /* down */
+        { 0x0310, KEY_9 },
+        { 0x0311, KEY_7 },
+        { 0x0314, KEY_VOLUMEUP },
+        { 0x0315, KEY_CHANNELUP },
+        { 0x0316, KEY_OK },
+        { 0x0317, KEY_POWER2 },
+        { 0x031a, KEY_1 },
+        { 0x031c, KEY_4 },
+        { 0x031d, KEY_6 },
+        { 0x031f, KEY_VOLUMEDOWN },
 };
 
 static struct rc_map_list msi_digivox_ii_map = {
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 699eef39128b..2ea913a44ae8 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -517,6 +517,9 @@ static int nvt_set_tx_carrier(struct rc_dev *dev, u32 carrier)
         struct nvt_dev *nvt = dev->priv;
         u16 val;
 
+        if (carrier == 0)
+                return -EINVAL;
+
         nvt_cir_reg_write(nvt, 1, CIR_CP);
         val = 3000000 / (carrier) - 1;
         nvt_cir_reg_write(nvt, val & 0xff, CIR_CC);
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index 49731b1a9c57..9f5a17bb5ef5 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -890,6 +890,9 @@ static int redrat3_set_tx_carrier(struct rc_dev *rcdev, u32 carrier)
         struct device *dev = rr3->dev;
 
         rr3_dbg(dev, "Setting modulation frequency to %u", carrier);
+        if (carrier == 0)
+                return -EINVAL;
+
         rr3->carrier = carrier;
 
         return carrier;
diff --git a/drivers/media/rc/winbond-cir.c b/drivers/media/rc/winbond-cir.c
index 30ae1f24abc3..7c9b5f33113b 100644
--- a/drivers/media/rc/winbond-cir.c
+++ b/drivers/media/rc/winbond-cir.c
@@ -184,7 +184,7 @@ enum wbcir_txstate {
 };
 
 /* Misc */
-#define WBCIR_NAME      "Winbond CIR"
+#define WBCIR_NAME      "winbond-cir"
 #define WBCIR_ID_FAMILY          0xF1 /* Family ID for the WPCD376I     */
 #define WBCIR_ID_CHIP            0x04 /* Chip ID for the WPCD376I       */
 #define INVALID_SCANCODE   0x7FFFFFFF /* Invalid with all protos        */
diff --git a/drivers/media/tuners/mt2063.c b/drivers/media/tuners/mt2063.c
index 0ed9091ff48e..2e1a02e360ff 100644
--- a/drivers/media/tuners/mt2063.c
+++ b/drivers/media/tuners/mt2063.c
@@ -245,7 +245,7 @@ struct mt2063_state {
 /*
  * mt2063_write - Write data into the I2C bus
  */
-static u32 mt2063_write(struct mt2063_state *state, u8 reg, u8 *data, u32 len)
+static int mt2063_write(struct mt2063_state *state, u8 reg, u8 *data, u32 len)
 {
         struct dvb_frontend *fe = state->frontend;
         int ret;
@@ -277,9 +277,9 @@ static u32 mt2063_write(struct mt2063_state *state, u8 reg, u8 *data, u32 len)
 /*
  * mt2063_write - Write register data into the I2C bus, caching the value
  */
-static u32 mt2063_setreg(struct mt2063_state *state, u8 reg, u8 val)
+static int mt2063_setreg(struct mt2063_state *state, u8 reg, u8 val)
 {
-        u32 status;
+        int status;
 
         dprintk(2, "\n");
 
@@ -298,10 +298,10 @@ static u32 mt2063_setreg(struct mt2063_state *state, u8 reg, u8 val)
 /*
  * mt2063_read - Read data from the I2C bus
  */
-static u32 mt2063_read(struct mt2063_state *state,
+static int mt2063_read(struct mt2063_state *state,
                            u8 subAddress, u8 *pData, u32 cnt)
 {
-        u32 status = 0; /* Status to be returned        */
+        int status = 0; /* Status to be returned        */
         struct dvb_frontend *fe = state->frontend;
         u32 i = 0;
 
@@ -816,7 +816,7 @@ static u32 IsSpurInBand(struct MT2063_AvoidSpursData_t *pAS_Info,
  */
 static u32 MT2063_AvoidSpurs(struct MT2063_AvoidSpursData_t *pAS_Info)
 {
-        u32 status = 0;
+        int status = 0;
         u32 fm, fp;             /*  restricted range on LO's        */
         pAS_Info->bSpurAvoided = 0;
         pAS_Info->nSpursFound = 0;
@@ -935,14 +935,14 @@ static u32 MT2063_AvoidSpurs(struct MT2063_AvoidSpursData_t *pAS_Info)
  *
  * This function returns 0, if no lock, 1 if locked and a value < 1 if error
  */
-static unsigned int mt2063_lockStatus(struct mt2063_state *state)
+static int mt2063_lockStatus(struct mt2063_state *state)
 {
         const u32 nMaxWait = 100;       /*  wait a maximum of 100 msec   */
         const u32 nPollRate = 2;        /*  poll status bits every 2 ms */
         const u32 nMaxLoops = nMaxWait / nPollRate;
         const u8 LO1LK = 0x80;
         u8 LO2LK = 0x08;
-        u32 status;
+        int status;
         u32 nDelays = 0;
 
         dprintk(2, "\n");
@@ -1069,7 +1069,7 @@ static u32 mt2063_get_dnc_output_enable(struct mt2063_state *state,
 static u32 mt2063_set_dnc_output_enable(struct mt2063_state *state,
                                         enum MT2063_DNC_Output_Enable nValue)
 {
-        u32 status = 0; /* Status to be returned        */
+        int status = 0; /* Status to be returned        */
         u8 val = 0;
 
         dprintk(2, "\n");
@@ -1203,7 +1203,7 @@ static u32 mt2063_set_dnc_output_enable(struct mt2063_state *state,
 static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
                                   enum mt2063_delivery_sys Mode)
 {
-        u32 status = 0; /* Status to be returned        */
+        int status = 0; /* Status to be returned        */
         u8 val;
         u32 longval;
 
@@ -1345,7 +1345,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 static u32 MT2063_ClearPowerMaskBits(struct mt2063_state *state,
                                      enum MT2063_Mask_Bits Bits)
 {
-        u32 status = 0;
+        int status = 0;
 
         dprintk(2, "\n");
         Bits = (enum MT2063_Mask_Bits)(Bits & MT2063_ALL_SD);   /* Only valid bits for this tuner */
@@ -1374,7 +1374,7 @@ static u32 MT2063_ClearPowerMaskBits(struct mt2063_state *state,
  */
 static u32 MT2063_SoftwareShutdown(struct mt2063_state *state, u8 Shutdown)
 {
-        u32 status;
+        int status;
 
         dprintk(2, "\n");
         if (Shutdown == 1)
@@ -1540,7 +1540,7 @@ static u32 FindClearTuneFilter(struct mt2063_state *state, u32 f_in)
 static u32 MT2063_Tune(struct mt2063_state *state, u32 f_in)
 {                               /* RF input center frequency   */
 
-        u32 status = 0;
+        int status = 0;
         u32 LO1;                /*  1st LO register value           */
         u32 Num1;               /*  Numerator for LO1 reg. value    */
         u32 f_IF1;              /*  1st IF requested                */
@@ -1803,7 +1803,7 @@ static const u8 MT2063B3_defaults[] = {
 
 static int mt2063_init(struct dvb_frontend *fe)
 {
-        u32 status;
+        int status;
         struct mt2063_state *state = fe->tuner_priv;
         u8 all_resets = 0xF0;   /* reset/load bits */
         const u8 *def = NULL;
@@ -2249,8 +2249,8 @@ struct dvb_frontend *mt2063_attach(struct dvb_frontend *fe,
         dprintk(2, "\n");
 
         state = kzalloc(sizeof(struct mt2063_state), GFP_KERNEL);
-        if (state == NULL)
-                goto error;
+        if (!state)
+                return NULL;
 
         state->config = config;
         state->i2c = i2c;
@@ -2261,18 +2261,15 @@ struct dvb_frontend *mt2063_attach(struct dvb_frontend *fe,
 
         printk(KERN_INFO "%s: Attaching MT2063\n", __func__);
         return fe;
-
-error:
-        kfree(state);
-        return NULL;
 }
 EXPORT_SYMBOL_GPL(mt2063_attach);
 
+#if 0
 /*
  * Ancillary routines visible outside mt2063
  * FIXME: Remove them in favor of using standard tuner callbacks
  */
-unsigned int tuner_MT2063_SoftwareShutdown(struct dvb_frontend *fe)
+static int tuner_MT2063_SoftwareShutdown(struct dvb_frontend *fe)
 {
         struct mt2063_state *state = fe->tuner_priv;
         int err = 0;
@@ -2285,9 +2282,8 @@ unsigned int tuner_MT2063_SoftwareShutdown(struct dvb_frontend *fe)
 
         return err;
 }
-EXPORT_SYMBOL_GPL(tuner_MT2063_SoftwareShutdown);
 
-unsigned int tuner_MT2063_ClearPowerMaskBits(struct dvb_frontend *fe)
+static int tuner_MT2063_ClearPowerMaskBits(struct dvb_frontend *fe)
 {
         struct mt2063_state *state = fe->tuner_priv;
         int err = 0;
@@ -2300,7 +2296,7 @@ unsigned int tuner_MT2063_ClearPowerMaskBits(struct dvb_frontend *fe)
 
         return err;
 }
-EXPORT_SYMBOL_GPL(tuner_MT2063_ClearPowerMaskBits);
+#endif
 
 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
 MODULE_DESCRIPTION("MT2063 Silicon tuner");
diff --git a/drivers/media/tuners/mt2063.h b/drivers/media/tuners/mt2063.h
index 3f5cfd93713f..ab24170c1571 100644
--- a/drivers/media/tuners/mt2063.h
+++ b/drivers/media/tuners/mt2063.h
@@ -23,10 +23,6 @@ static inline struct dvb_frontend *mt2063_attach(struct dvb_frontend *fe,
         return NULL;
 }
 
-/* FIXME: Should use the standard DVB attachment interfaces */
-unsigned int tuner_MT2063_SoftwareShutdown(struct dvb_frontend *fe);
-unsigned int tuner_MT2063_ClearPowerMaskBits(struct dvb_frontend *fe);
-
 #endif /* CONFIG_DVB_MT2063 */
 
 #endif /* __MT2063_H__ */
diff --git a/drivers/media/tuners/tda18271-common.c b/drivers/media/tuners/tda18271-common.c
index 221171eeb0c3..18c77afe2e4f 100644
--- a/drivers/media/tuners/tda18271-common.c
+++ b/drivers/media/tuners/tda18271-common.c
@@ -187,7 +187,8 @@ int tda18271_read_extended(struct dvb_frontend *fe)
         return (ret == 2 ? 0 : ret);
 }
 
-int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
+static int __tda18271_write_regs(struct dvb_frontend *fe, int idx, int len,
+                        bool lock_i2c)
 {
         struct tda18271_priv *priv = fe->tuner_priv;
         unsigned char *regs = priv->tda18271_regs;
@@ -198,7 +199,6 @@ int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
 
         BUG_ON((len == 0) || (idx + len > sizeof(buf)));
 
-
         switch (priv->small_i2c) {
         case TDA18271_03_BYTE_CHUNK_INIT:
                 max = 3;
@@ -214,7 +214,19 @@ int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
                 max = 39;
         }
 
-        tda18271_i2c_gate_ctrl(fe, 1);
+
+        /*
+         * If lock_i2c is true, it will take the I2C bus for tda18271 private
+         * usage during the entire write ops, as otherwise, bad things could
+         * happen.
+         * During device init, several write operations will happen. So,
+         * tda18271_init_regs controls the I2C lock directly,
+         * disabling lock_i2c here.
+         */
+        if (lock_i2c) {
+                tda18271_i2c_gate_ctrl(fe, 1);
+                i2c_lock_adapter(priv->i2c_props.adap);
+        }
         while (len) {
                 if (max > len)
                         max = len;
@@ -226,14 +238,17 @@ int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
                 msg.len = max + 1;
 
                 /* write registers */
-                ret = i2c_transfer(priv->i2c_props.adap, &msg, 1);
+                ret = __i2c_transfer(priv->i2c_props.adap, &msg, 1);
                 if (ret != 1)
                         break;
 
                 idx += max;
                 len -= max;
         }
-        tda18271_i2c_gate_ctrl(fe, 0);
+        if (lock_i2c) {
+                i2c_unlock_adapter(priv->i2c_props.adap);
+                tda18271_i2c_gate_ctrl(fe, 0);
+        }
 
         if (ret != 1)
                 tda_err("ERROR: idx = 0x%x, len = %d, "
@@ -242,10 +257,16 @@ int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
         return (ret == 1 ? 0 : ret);
 }
 
+int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
+{
+        return __tda18271_write_regs(fe, idx, len, true);
+}
+
 /*---------------------------------------------------------------------*/
 
-int tda18271_charge_pump_source(struct dvb_frontend *fe,
-                                enum tda18271_pll pll, int force)
+static int __tda18271_charge_pump_source(struct dvb_frontend *fe,
+                                         enum tda18271_pll pll, int force,
+                                         bool lock_i2c)
 {
         struct tda18271_priv *priv = fe->tuner_priv;
         unsigned char *regs = priv->tda18271_regs;
@@ -255,9 +276,16 @@ int tda18271_charge_pump_source(struct dvb_frontend *fe,
         regs[r_cp] &= ~0x20;
         regs[r_cp] |= ((force & 1) << 5);
 
-        return tda18271_write_regs(fe, r_cp, 1);
+        return __tda18271_write_regs(fe, r_cp, 1, lock_i2c);
+}
+
+int tda18271_charge_pump_source(struct dvb_frontend *fe,
+                                enum tda18271_pll pll, int force)
+{
+        return __tda18271_charge_pump_source(fe, pll, force, true);
 }
 
+
 int tda18271_init_regs(struct dvb_frontend *fe)
 {
         struct tda18271_priv *priv = fe->tuner_priv;
@@ -267,6 +295,13 @@ int tda18271_init_regs(struct dvb_frontend *fe)
                 i2c_adapter_id(priv->i2c_props.adap),
                 priv->i2c_props.addr);
 
+        /*
+         * Don't let any other I2C transfer to happen at adapter during init,
+         * as those could cause bad things
+         */
+        tda18271_i2c_gate_ctrl(fe, 1);
+        i2c_lock_adapter(priv->i2c_props.adap);
+
         /* initialize registers */
         switch (priv->id) {
         case TDA18271HDC1:
@@ -352,28 +387,28 @@ int tda18271_init_regs(struct dvb_frontend *fe)
         regs[R_EB22] = 0x48;
         regs[R_EB23] = 0xb0;
 
-        tda18271_write_regs(fe, 0x00, TDA18271_NUM_REGS);
+        __tda18271_write_regs(fe, 0x00, TDA18271_NUM_REGS, false);
 
         /* setup agc1 gain */
         regs[R_EB17] = 0x00;
-        tda18271_write_regs(fe, R_EB17, 1);
+        __tda18271_write_regs(fe, R_EB17, 1, false);
         regs[R_EB17] = 0x03;
-        tda18271_write_regs(fe, R_EB17, 1);
+        __tda18271_write_regs(fe, R_EB17, 1, false);
         regs[R_EB17] = 0x43;
-        tda18271_write_regs(fe, R_EB17, 1);
+        __tda18271_write_regs(fe, R_EB17, 1, false);
         regs[R_EB17] = 0x4c;
-        tda18271_write_regs(fe, R_EB17, 1);
+        __tda18271_write_regs(fe, R_EB17, 1, false);
 
         /* setup agc2 gain */
         if ((priv->id) == TDA18271HDC1) {
                 regs[R_EB20] = 0xa0;
-                tda18271_write_regs(fe, R_EB20, 1);
+                __tda18271_write_regs(fe, R_EB20, 1, false);
                 regs[R_EB20] = 0xa7;
-                tda18271_write_regs(fe, R_EB20, 1);
+                __tda18271_write_regs(fe, R_EB20, 1, false);
                 regs[R_EB20] = 0xe7;
-                tda18271_write_regs(fe, R_EB20, 1);
+                __tda18271_write_regs(fe, R_EB20, 1, false);
                 regs[R_EB20] = 0xec;
-                tda18271_write_regs(fe, R_EB20, 1);
+                __tda18271_write_regs(fe, R_EB20, 1, false);
         }
 
         /* image rejection calibration */
@@ -391,21 +426,21 @@ int tda18271_init_regs(struct dvb_frontend *fe)
         regs[R_MD2] = 0x08;
         regs[R_MD3] = 0x00;
 
-        tda18271_write_regs(fe, R_EP3, 11);
+        __tda18271_write_regs(fe, R_EP3, 11, false);
 
         if ((priv->id) == TDA18271HDC2) {
                 /* main pll cp source on */
-                tda18271_charge_pump_source(fe, TDA18271_MAIN_PLL, 1);
+                __tda18271_charge_pump_source(fe, TDA18271_MAIN_PLL, 1, false);
                 msleep(1);
 
                 /* main pll cp source off */
-                tda18271_charge_pump_source(fe, TDA18271_MAIN_PLL, 0);
+                __tda18271_charge_pump_source(fe, TDA18271_MAIN_PLL, 0, false);
         }
 
         msleep(5); /* pll locking */
 
         /* launch detector */
-        tda18271_write_regs(fe, R_EP1, 1);
+        __tda18271_write_regs(fe, R_EP1, 1, false);
         msleep(5); /* wanted low measurement */
 
         regs[R_EP5] = 0x85;
@@ -413,11 +448,11 @@ int tda18271_init_regs(struct dvb_frontend *fe)
         regs[R_CD1] = 0x66;
         regs[R_CD2] = 0x70;
 
-        tda18271_write_regs(fe, R_EP3, 7);
+        __tda18271_write_regs(fe, R_EP3, 7, false);
         msleep(5); /* pll locking */
 
         /* launch optimization algorithm */
-        tda18271_write_regs(fe, R_EP2, 1);
+        __tda18271_write_regs(fe, R_EP2, 1, false);
         msleep(30); /* image low optimization completion */
 
         /* mid-band */
@@ -428,11 +463,11 @@ int tda18271_init_regs(struct dvb_frontend *fe)
         regs[R_MD1] = 0x73;
         regs[R_MD2] = 0x1a;
 
-        tda18271_write_regs(fe, R_EP3, 11);
+        __tda18271_write_regs(fe, R_EP3, 11, false);
         msleep(5); /* pll locking */
 
         /* launch detector */
-        tda18271_write_regs(fe, R_EP1, 1);
+        __tda18271_write_regs(fe, R_EP1, 1, false);
         msleep(5); /* wanted mid measurement */
 
         regs[R_EP5] = 0x86;
@@ -440,11 +475,11 @@ int tda18271_init_regs(struct dvb_frontend *fe)
         regs[R_CD1] = 0x66;
         regs[R_CD2] = 0xa0;
 
-        tda18271_write_regs(fe, R_EP3, 7);
+        __tda18271_write_regs(fe, R_EP3, 7, false);
         msleep(5); /* pll locking */
 
         /* launch optimization algorithm */
-        tda18271_write_regs(fe, R_EP2, 1);
+        __tda18271_write_regs(fe, R_EP2, 1, false);
         msleep(30); /* image mid optimization completion */
 
         /* high-band */
@@ -456,30 +491,33 @@ int tda18271_init_regs(struct dvb_frontend *fe)
         regs[R_MD1] = 0x71;
         regs[R_MD2] = 0xcd;
 
-        tda18271_write_regs(fe, R_EP3, 11);
+        __tda18271_write_regs(fe, R_EP3, 11, false);
         msleep(5); /* pll locking */
 
         /* launch detector */
-        tda18271_write_regs(fe, R_EP1, 1);
+        __tda18271_write_regs(fe, R_EP1, 1, false);
         msleep(5); /* wanted high measurement */
 
         regs[R_EP5] = 0x87;
         regs[R_CD1] = 0x65;
         regs[R_CD2] = 0x50;
 
-        tda18271_write_regs(fe, R_EP3, 7);
+        __tda18271_write_regs(fe, R_EP3, 7, false);
         msleep(5); /* pll locking */
 
         /* launch optimization algorithm */
-        tda18271_write_regs(fe, R_EP2, 1);
+        __tda18271_write_regs(fe, R_EP2, 1, false);
         msleep(30); /* image high optimization completion */
 
         /* return to normal mode */
         regs[R_EP4] = 0x64;
-        tda18271_write_regs(fe, R_EP4, 1);
+        __tda18271_write_regs(fe, R_EP4, 1, false);
 
         /* synchronize */
-        tda18271_write_regs(fe, R_EP1, 1);
+        __tda18271_write_regs(fe, R_EP1, 1, false);
+
+        i2c_unlock_adapter(priv->i2c_props.adap);
+        tda18271_i2c_gate_ctrl(fe, 0);
 
         return 0;
 }
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.c b/drivers/media/usb/dvb-usb-v2/af9015.c
index 824f1911ee21..3d7526e28d42 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.c
+++ b/drivers/media/usb/dvb-usb-v2/af9015.c
@@ -500,7 +500,7 @@ static int af9015_read_config(struct dvb_usb_device *d)
                 case 3:
                         state->af9013_config[i].clock = 25000000;
                         break;
-                };
+                }
                 dev_dbg(&d->udev->dev, "%s: [%d] xtal=%d set clock=%d\n",
                                 __func__, i, val,
                                 state->af9013_config[i].clock);
@@ -568,7 +568,7 @@ static int af9015_read_config(struct dvb_usb_device *d)
                                         "supported, please report!\n",
                                         KBUILD_MODNAME, val);
                         return -ENODEV;
-                };
+                }
 
                 state->af9013_config[i].tuner = val;
                 dev_dbg(&d->udev->dev, "%s: [%d] tuner id=%d\n",
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.c b/drivers/media/usb/dvb-usb-v2/af9035.c
index aabd3fc03ea7..ea27eaff4e34 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.c
+++ b/drivers/media/usb/dvb-usb-v2/af9035.c
@@ -520,7 +520,7 @@ static int af9035_read_config(struct dvb_usb_device *d)
                         dev_warn(&d->udev->dev, "%s: tuner id=%02x not " \
                                         "supported, please report!",
                                         KBUILD_MODNAME, tmp);
-                };
+                }
 
                 /* tuner IF frequency */
                 ret = af9035_rd_reg(d, EEPROM_1_IFFREQ_L + eeprom_shift, &tmp);
diff --git a/drivers/media/usb/dvb-usb/a800.c b/drivers/media/usb/dvb-usb/a800.c
index 8d7fef84afd8..83684ed023cd 100644
--- a/drivers/media/usb/dvb-usb/a800.c
+++ b/drivers/media/usb/dvb-usb/a800.c
@@ -93,7 +93,7 @@ static int a800_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
         /* call the universal NEC remote processor, to find out the key's state and event */
         dvb_usb_nec_rc_key_to_event(d,key,event,state);
         if (key[0] != 0)
-                deb_rc("key: %x %x %x %x %x\n",key[0],key[1],key[2],key[3],key[4]);
+                deb_rc("key: %*ph\n", 5, key);
         ret = 0;
 out:
         kfree(key);
diff --git a/drivers/media/usb/dvb-usb/cinergyT2-core.c b/drivers/media/usb/dvb-usb/cinergyT2-core.c
index 0a98548ecd17..9fd1527494eb 100644
--- a/drivers/media/usb/dvb-usb/cinergyT2-core.c
+++ b/drivers/media/usb/dvb-usb/cinergyT2-core.c
@@ -172,8 +172,7 @@ static int cinergyt2_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
                 if (*event != d->last_event)
                         st->rc_counter = 0;
 
-                deb_rc("key: %x %x %x %x %x\n",
-                       key[0], key[1], key[2], key[3], key[4]);
+                deb_rc("key: %*ph\n", 5, key);
         }
         return 0;
 }
diff --git a/drivers/media/usb/dvb-usb/dibusb-common.c b/drivers/media/usb/dvb-usb/dibusb-common.c
index a76bbb29ca36..af0d4321845b 100644
--- a/drivers/media/usb/dvb-usb/dibusb-common.c
+++ b/drivers/media/usb/dvb-usb/dibusb-common.c
@@ -473,7 +473,7 @@ int dibusb_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
         dvb_usb_generic_rw(d,&cmd,1,key,5,0);
         dvb_usb_nec_rc_key_to_event(d,key,event,state);
         if (key[0] != 0)
-                deb_info("key: %x %x %x %x %x\n",key[0],key[1],key[2],key[3],key[4]);
+                deb_info("key: %*ph\n", 5, key);
         return 0;
 }
 EXPORT_SYMBOL(dibusb_rc_query);
diff --git a/drivers/media/usb/dvb-usb/digitv.c b/drivers/media/usb/dvb-usb/digitv.c
index ff34419a4c88..772bde3c5020 100644
--- a/drivers/media/usb/dvb-usb/digitv.c
+++ b/drivers/media/usb/dvb-usb/digitv.c
@@ -253,7 +253,7 @@ static int digitv_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
         }
 
         if (key[0] != 0)
-                deb_rc("key: %x %x %x %x %x\n",key[0],key[1],key[2],key[3],key[4]);
+                deb_rc("key: %*ph\n", 5, key);
         return 0;
 }
 
diff --git a/drivers/media/usb/dvb-usb/dtt200u.c b/drivers/media/usb/dvb-usb/dtt200u.c
index 66f205c112b2..c357fb3b0a88 100644
--- a/drivers/media/usb/dvb-usb/dtt200u.c
+++ b/drivers/media/usb/dvb-usb/dtt200u.c
@@ -84,7 +84,7 @@ static int dtt200u_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
         dvb_usb_generic_rw(d,&cmd,1,key,5,0);
         dvb_usb_nec_rc_key_to_event(d,key,event,state);
         if (key[0] != 0)
-                deb_info("key: %x %x %x %x %x\n",key[0],key[1],key[2],key[3],key[4]);
+                deb_info("key: %*ph\n", 5, key);
         return 0;
 }
 
diff --git a/drivers/media/usb/dvb-usb/m920x.c b/drivers/media/usb/dvb-usb/m920x.c
index 288af29a8bb7..661bb75be955 100644
--- a/drivers/media/usb/dvb-usb/m920x.c
+++ b/drivers/media/usb/dvb-usb/m920x.c
@@ -358,7 +358,7 @@ static int m920x_firmware_download(struct usb_device *udev, const struct firmwar
 
         if ((ret = m920x_read(udev, M9206_FILTER, 0x0, 0x8000, read, 4)) != 0)
                 goto done;
-        deb("%x %x %x %x\n", read[0], read[1], read[2], read[3]);
+        deb("%*ph\n", 4, read);
 
         if ((ret = m920x_read(udev, M9206_FW, 0x0, 0x0, read, 1)) != 0)
                 goto done;
diff --git a/drivers/media/usb/dvb-usb/technisat-usb2.c b/drivers/media/usb/dvb-usb/technisat-usb2.c
index acefaa89cc53..7a8c8c18590f 100644
--- a/drivers/media/usb/dvb-usb/technisat-usb2.c
+++ b/drivers/media/usb/dvb-usb/technisat-usb2.c
@@ -677,6 +677,7 @@ static struct usb_device_id technisat_usb2_id_table[] = {
         { USB_DEVICE(USB_VID_TECHNISAT, USB_PID_TECHNISAT_USB2_DVB_S2) },
         { 0 }           /* Terminating entry */
 };
+MODULE_DEVICE_TABLE(usb, technisat_usb2_id_table);
 
 /* device description */
 static struct dvb_usb_device_properties technisat_usb2_devices = {
diff --git a/drivers/media/usb/em28xx/em28xx-cards.c b/drivers/media/usb/em28xx/em28xx-cards.c
index f7297ae76b48..16a84f9f46d8 100644
--- a/drivers/media/usb/em28xx/em28xx-cards.c
+++ b/drivers/media/usb/em28xx/em28xx-cards.c
@@ -2203,7 +2203,7 @@ EXPORT_SYMBOL_GPL(em28xx_tuner_callback);
 
 static inline void em28xx_set_model(struct em28xx *dev)
 {
-        memcpy(&dev->board, &em28xx_boards[dev->model], sizeof(dev->board));
+        dev->board = em28xx_boards[dev->model];
 
         /* Those are the default values for the majority of boards
            Use those values if not specified otherwise at boards entry
diff --git a/drivers/media/usb/em28xx/em28xx-dvb.c b/drivers/media/usb/em28xx/em28xx-dvb.c
index 913e5227897a..13ae821949e9 100644
--- a/drivers/media/usb/em28xx/em28xx-dvb.c
+++ b/drivers/media/usb/em28xx/em28xx-dvb.c
@@ -574,18 +574,19 @@ static void pctv_520e_init(struct em28xx *dev)
                 i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
 };
 
-static int em28xx_pctv_290e_set_lna(struct dvb_frontend *fe, int val)
+static int em28xx_pctv_290e_set_lna(struct dvb_frontend *fe)
 {
+        struct dtv_frontend_properties *c = &fe->dtv_property_cache;
         struct em28xx *dev = fe->dvb->priv;
 #ifdef CONFIG_GPIOLIB
         struct em28xx_dvb *dvb = dev->dvb;
         int ret;
         unsigned long flags;
 
-        if (val)
-                flags = GPIOF_OUT_INIT_LOW;
+        if (c->lna == 1)
+                flags = GPIOF_OUT_INIT_HIGH; /* enable LNA */
         else
-                flags = GPIOF_OUT_INIT_HIGH;
+                flags = GPIOF_OUT_INIT_LOW; /* disable LNA */
 
         ret = gpio_request_one(dvb->lna_gpio, flags, NULL);
         if (ret)
@@ -595,8 +596,8 @@ static int em28xx_pctv_290e_set_lna(struct dvb_frontend *fe, int val)
 
         return ret;
 #else
-        dev_warn(&dev->udev->dev, "%s: LNA control is disabled\n",
-                        KBUILD_MODNAME);
+        dev_warn(&dev->udev->dev, "%s: LNA control is disabled (lna=%u)\n",
+                        KBUILD_MODNAME, c->lna);
         return 0;
 #endif
 }
diff --git a/drivers/media/usb/stk1160/stk1160-core.c b/drivers/media/usb/stk1160/stk1160-core.c
index b62740846061..34a26e0cfe77 100644
--- a/drivers/media/usb/stk1160/stk1160-core.c
+++ b/drivers/media/usb/stk1160/stk1160-core.c
@@ -100,12 +100,21 @@ int stk1160_write_reg(struct stk1160 *dev, u16 reg, u16 value)
 
 void stk1160_select_input(struct stk1160 *dev)
 {
+        int route;
         static const u8 gctrl[] = {
-                0x98, 0x90, 0x88, 0x80
+                0x98, 0x90, 0x88, 0x80, 0x98
         };
 
-        if (dev->ctl_input < ARRAY_SIZE(gctrl))
+        if (dev->ctl_input == STK1160_SVIDEO_INPUT)
+                route = SAA7115_SVIDEO3;
+        else
+                route = SAA7115_COMPOSITE0;
+
+        if (dev->ctl_input < ARRAY_SIZE(gctrl)) {
+                v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_routing,
+                                route, 0, 0);
                 stk1160_write_reg(dev, STK1160_GCTRL, gctrl[dev->ctl_input]);
+        }
 }
 
 /* TODO: We should break this into pieces */
@@ -351,8 +360,6 @@ static int stk1160_probe(struct usb_interface *interface,
 
         /* i2c reset saa711x */
         v4l2_device_call_all(&dev->v4l2_dev, 0, core, reset, 0);
-        v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_routing,
-                                0, 0, 0);
         v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_stream, 0);
 
         /* reset stk1160 to default values */
diff --git a/drivers/media/usb/stk1160/stk1160-v4l.c b/drivers/media/usb/stk1160/stk1160-v4l.c
index fe6e857969ca..6694f9e2ca57 100644
--- a/drivers/media/usb/stk1160/stk1160-v4l.c
+++ b/drivers/media/usb/stk1160/stk1160-v4l.c
@@ -419,7 +419,12 @@ static int vidioc_enum_input(struct file *file, void *priv,
         if (i->index > STK1160_MAX_INPUT)
                 return -EINVAL;
 
-        sprintf(i->name, "Composite%d", i->index);
+        /* S-Video special handling */
+        if (i->index == STK1160_SVIDEO_INPUT)
+                sprintf(i->name, "S-Video");
+        else
+                sprintf(i->name, "Composite%d", i->index);
+
         i->type = V4L2_INPUT_TYPE_CAMERA;
         i->std = dev->vdev.tvnorms;
         return 0;
diff --git a/drivers/media/usb/stk1160/stk1160.h b/drivers/media/usb/stk1160/stk1160.h
index 3feba0033f98..68c8707d36ab 100644
--- a/drivers/media/usb/stk1160/stk1160.h
+++ b/drivers/media/usb/stk1160/stk1160.h
@@ -46,7 +46,8 @@
 
 #define STK1160_MIN_PKT_SIZE 3072
 
-#define STK1160_MAX_INPUT 3
+#define STK1160_MAX_INPUT 4
+#define STK1160_SVIDEO_INPUT 4
 
 #define STK1160_I2C_TIMEOUT 100
 
diff --git a/drivers/media/usb/uvc/uvc_queue.c b/drivers/media/usb/uvc/uvc_queue.c
index 5577381b5bf0..18a91fae6bc1 100644
--- a/drivers/media/usb/uvc/uvc_queue.c
+++ b/drivers/media/usb/uvc/uvc_queue.c
@@ -122,21 +122,27 @@ static struct vb2_ops uvc_queue_qops = {
         .buf_finish = uvc_buffer_finish,
 };
 
-void uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type,
+int uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type,
                     int drop_corrupted)
 {
+        int ret;
+
         queue->queue.type = type;
         queue->queue.io_modes = VB2_MMAP | VB2_USERPTR;
         queue->queue.drv_priv = queue;
         queue->queue.buf_struct_size = sizeof(struct uvc_buffer);
         queue->queue.ops = &uvc_queue_qops;
         queue->queue.mem_ops = &vb2_vmalloc_memops;
-        vb2_queue_init(&queue->queue);
+        ret = vb2_queue_init(&queue->queue);
+        if (ret)
+                return ret;
 
         mutex_init(&queue->mutex);
         spin_lock_init(&queue->irqlock);
         INIT_LIST_HEAD(&queue->irqqueue);
         queue->flags = drop_corrupted ? UVC_QUEUE_DROP_CORRUPTED : 0;
+
+        return 0;
 }
 
 /* -----------------------------------------------------------------------------
diff --git a/drivers/media/usb/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c
index 1c15b4227bdb..57c3076a4625 100644
--- a/drivers/media/usb/uvc/uvc_video.c
+++ b/drivers/media/usb/uvc/uvc_video.c
@@ -1755,7 +1755,9 @@ int uvc_video_init(struct uvc_streaming *stream)
         atomic_set(&stream->active, 0);
 
         /* Initialize the video buffers queue. */
-        uvc_queue_init(&stream->queue, stream->type, !uvc_no_drop_param);
+        ret = uvc_queue_init(&stream->queue, stream->type, !uvc_no_drop_param);
+        if (ret)
+                return ret;
 
         /* Alternate setting 0 should be the default, yet the XBox Live Vision
          * Cam (and possibly other devices) crash or otherwise misbehave if
diff --git a/drivers/media/usb/uvc/uvcvideo.h b/drivers/media/usb/uvc/uvcvideo.h
index 3764040475bb..af216ec45e39 100644
--- a/drivers/media/usb/uvc/uvcvideo.h
+++ b/drivers/media/usb/uvc/uvcvideo.h
@@ -600,7 +600,7 @@ extern struct uvc_driver uvc_driver;
 extern struct uvc_entity *uvc_entity_by_id(struct uvc_device *dev, int id);
 
 /* Video buffers queue management. */
-extern void uvc_queue_init(struct uvc_video_queue *queue,
+extern int uvc_queue_init(struct uvc_video_queue *queue,
                 enum v4l2_buf_type type, int drop_corrupted);
 extern int uvc_alloc_buffers(struct uvc_video_queue *queue,
                 struct v4l2_requestbuffers *rb);
diff --git a/drivers/media/v4l2-core/v4l2-ctrls.c b/drivers/media/v4l2-core/v4l2-ctrls.c
index 631cdc0e0bda..f6ee201d9347 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls.c
@@ -384,6 +384,25 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
                 "Extended SAR",
                 NULL,
         };
+        static const char * const h264_fp_arrangement_type[] = {
+                "Checkerboard",
+                "Column",
+                "Row",
+                "Side by Side",
+                "Top Bottom",
+                "Temporal",
+                NULL,
+        };
+        static const char * const h264_fmo_map_type[] = {
+                "Interleaved Slices",
+                "Scattered Slices",
+                "Foreground with Leftover",
+                "Box Out",
+                "Raster Scan",
+                "Wipe Scan",
+                "Explicit",
+                NULL,
+        };
         static const char * const mpeg_mpeg4_level[] = {
                 "0",
                 "0b",
@@ -508,6 +527,10 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
                 return h264_profile;
         case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:
                 return vui_sar_idc;
+        case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE:
+                return h264_fp_arrangement_type;
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_MAP_TYPE:
+                return h264_fmo_map_type;
         case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
                 return mpeg_mpeg4_level;
         case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
@@ -643,6 +666,22 @@ const char *v4l2_ctrl_get_name(u32 id)
         case V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_WIDTH:        return "Horizontal Size of SAR";
         case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE:           return "Aspect Ratio VUI Enable";
         case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:              return "VUI Aspect Ratio IDC";
+        case V4L2_CID_MPEG_VIDEO_H264_SEI_FRAME_PACKING:        return "H264 Enable Frame Packing SEI";
+        case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_CURRENT_FRAME_0:   return "H264 Set Curr. Frame as Frame0";
+        case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE:  return "H264 FP Arrangement Type";
+        case V4L2_CID_MPEG_VIDEO_H264_FMO:                      return "H264 Flexible MB Ordering";
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_MAP_TYPE:             return "H264 Map Type for FMO";
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_SLICE_GROUP:          return "H264 FMO Number of Slice Groups";
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_CHANGE_DIRECTION:     return "H264 FMO Direction of Change";
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_CHANGE_RATE:          return "H264 FMO Size of 1st Slice Grp";
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_RUN_LENGTH:           return "H264 FMO No. of Consecutive MBs";
+        case V4L2_CID_MPEG_VIDEO_H264_ASO:                      return "H264 Arbitrary Slice Ordering";
+        case V4L2_CID_MPEG_VIDEO_H264_ASO_SLICE_ORDER:          return "H264 ASO Slice Order";
+        case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING:      return "Enable H264 Hierarchical Coding";
+        case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_TYPE: return "H264 Hierarchical Coding Type";
+        case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_LAYER:return "H264 Number of HC Layers";
+        case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_LAYER_QP:
+                                                                return "H264 Set QP Value for HC Layers";
         case V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP:              return "MPEG4 I-Frame QP Value";
         case V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP:              return "MPEG4 P-Frame QP Value";
         case V4L2_CID_MPEG_VIDEO_MPEG4_B_FRAME_QP:              return "MPEG4 B-Frame QP Value";
@@ -657,6 +696,7 @@ const char *v4l2_ctrl_get_name(u32 id)
         case V4L2_CID_MPEG_VIDEO_VBV_SIZE:                      return "VBV Buffer Size";
         case V4L2_CID_MPEG_VIDEO_DEC_PTS:                       return "Video Decoder PTS";
         case V4L2_CID_MPEG_VIDEO_DEC_FRAME:                     return "Video Decoder Frame Count";
+        case V4L2_CID_MPEG_VIDEO_VBV_DELAY:                     return "Initial Delay for VBV Control";
 
         /* CAMERA controls */
         /* Keep the order of the 'case's the same as in videodev2.h! */
@@ -749,6 +789,7 @@ const char *v4l2_ctrl_get_name(u32 id)
         case V4L2_CID_IMAGE_PROC_CLASS:         return "Image Processing Controls";
         case V4L2_CID_LINK_FREQ:                return "Link Frequency";
         case V4L2_CID_PIXEL_RATE:               return "Pixel Rate";
+        case V4L2_CID_TEST_PATTERN:             return "Test Pattern";
 
         /* DV controls */
         case V4L2_CID_DV_CLASS:                 return "Digital Video Controls";
@@ -853,6 +894,8 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
         case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:
         case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
         case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:
+        case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE:
+        case V4L2_CID_MPEG_VIDEO_H264_FMO_MAP_TYPE:
         case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
         case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
         case V4L2_CID_JPEG_CHROMA_SUBSAMPLING:
@@ -862,6 +905,7 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
         case V4L2_CID_DV_TX_MODE:
         case V4L2_CID_DV_TX_RGB_RANGE:
         case V4L2_CID_DV_RX_RGB_RANGE:
+        case V4L2_CID_TEST_PATTERN:
                 *type = V4L2_CTRL_TYPE_MENU;
                 break;
         case V4L2_CID_LINK_FREQ:
@@ -1648,6 +1692,36 @@ struct v4l2_ctrl *v4l2_ctrl_new_std_menu(struct v4l2_ctrl_handler *hdl,
 }
 EXPORT_SYMBOL(v4l2_ctrl_new_std_menu);
 
+/* Helper function for standard menu controls with driver defined menu */
+struct v4l2_ctrl *v4l2_ctrl_new_std_menu_items(struct v4l2_ctrl_handler *hdl,
+                        const struct v4l2_ctrl_ops *ops, u32 id, s32 max,
+                        s32 mask, s32 def, const char * const *qmenu)
+{
+        enum v4l2_ctrl_type type;
+        const char *name;
+        u32 flags;
+        s32 step;
+        s32 min;
+
+        /* v4l2_ctrl_new_std_menu_items() should only be called for
+         * standard controls without a standard menu.
+         */
+        if (v4l2_ctrl_get_menu(id)) {
+                handler_set_err(hdl, -EINVAL);
+                return NULL;
+        }
+
+        v4l2_ctrl_fill(id, &name, &type, &min, &max, &step, &def, &flags);
+        if (type != V4L2_CTRL_TYPE_MENU || qmenu == NULL) {
+                handler_set_err(hdl, -EINVAL);
+                return NULL;
+        }
+        return v4l2_ctrl_new(hdl, ops, id, name, type, 0, max, mask, def,
+                             flags, qmenu, NULL, NULL);
+
+}
+EXPORT_SYMBOL(v4l2_ctrl_new_std_menu_items);
+
 /* Helper function for standard integer menu controls */
 struct v4l2_ctrl *v4l2_ctrl_new_int_menu(struct v4l2_ctrl_handler *hdl,
                         const struct v4l2_ctrl_ops *ops,
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index 9d3e46c446ad..8f388ff31ebb 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -157,8 +157,7 @@ static const char *v4l2_memory_names[] = {
         [V4L2_MEMORY_OVERLAY] = "overlay",
 };
 
-#define prt_names(a, arr) ((((a) >= 0) && ((a) < ARRAY_SIZE(arr))) ? \
-                           arr[a] : "unknown")
+#define prt_names(a, arr) (((unsigned)(a)) < ARRAY_SIZE(arr) ? arr[a] : "unknown")
 
 /* ------------------------------------------------------------------ */
 /* debug help functions                                               */
@@ -2188,6 +2187,7 @@ static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
         int ret = 0;
 
         switch (cmd) {
+        case VIDIOC_PREPARE_BUF:
         case VIDIOC_QUERYBUF:
         case VIDIOC_QBUF:
         case VIDIOC_DQBUF: {
@@ -2211,6 +2211,10 @@ static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
                 struct v4l2_subdev_edid *edid = parg;
 
                 if (edid->blocks) {
+                        if (edid->blocks > 256) {
+                                ret = -EINVAL;
+                                break;
+                        }
                         *user_ptr = (void __user *)edid->edid;
                         *kernel_ptr = (void *)&edid->edid;
                         *array_size = edid->blocks * 128;
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index e6a26b433e87..432df119af27 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -276,6 +276,9 @@ static void __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
  */
 static int __verify_planes_array(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 {
+        if (!V4L2_TYPE_IS_MULTIPLANAR(b->type))
+                return 0;
+
         /* Is memory for copying plane information present? */
         if (NULL == b->m.planes) {
                 dprintk(1, "Multi-planar buffer passed but "
@@ -331,10 +334,9 @@ static bool __buffers_in_use(struct vb2_queue *q)
  * __fill_v4l2_buffer() - fill in a struct v4l2_buffer with information to be
  * returned to userspace
  */
-static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
+static void __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 {
         struct vb2_queue *q = vb->vb2_queue;
-        int ret;
 
         /* Copy back data such as timestamp, flags, etc. */
         memcpy(b, &vb->v4l2_buf, offsetof(struct v4l2_buffer, m));
@@ -342,14 +344,11 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
         b->reserved = vb->v4l2_buf.reserved;
 
         if (V4L2_TYPE_IS_MULTIPLANAR(q->type)) {
-                ret = __verify_planes_array(vb, b);
-                if (ret)
-                        return ret;
-
                 /*
                  * Fill in plane-related data if userspace provided an array
-                 * for it. The memory and size is verified above.
+                 * for it. The caller has already verified memory and size.
                  */
+                b->length = vb->num_planes;
                 memcpy(b->m.planes, vb->v4l2_planes,
                         b->length * sizeof(struct v4l2_plane));
         } else {
@@ -391,8 +390,6 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 
         if (__buffer_in_use(q, vb))
                 b->flags |= V4L2_BUF_FLAG_MAPPED;
-
-        return 0;
 }
 
 /**
@@ -411,6 +408,7 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 int vb2_querybuf(struct vb2_queue *q, struct v4l2_buffer *b)
 {
         struct vb2_buffer *vb;
+        int ret;
 
         if (b->type != q->type) {
                 dprintk(1, "querybuf: wrong buffer type\n");
@@ -422,8 +420,10 @@ int vb2_querybuf(struct vb2_queue *q, struct v4l2_buffer *b)
                 return -EINVAL;
         }
         vb = q->bufs[b->index];
-
-        return __fill_v4l2_buffer(vb, b);
+        ret = __verify_planes_array(vb, b);
+        if (!ret)
+                __fill_v4l2_buffer(vb, b);
+        return ret;
 }
 EXPORT_SYMBOL(vb2_querybuf);
 
@@ -813,24 +813,16 @@ void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 EXPORT_SYMBOL_GPL(vb2_buffer_done);
 
 /**
- * __fill_vb2_buffer() - fill a vb2_buffer with information provided in
- * a v4l2_buffer by the userspace
+ * __fill_vb2_buffer() - fill a vb2_buffer with information provided in a
+ * v4l2_buffer by the userspace. The caller has already verified that struct
+ * v4l2_buffer has a valid number of planes.
  */
-static int __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b,
+static void __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b,
                                 struct v4l2_plane *v4l2_planes)
 {
         unsigned int plane;
-        int ret;
 
         if (V4L2_TYPE_IS_MULTIPLANAR(b->type)) {
-                /*
-                 * Verify that the userspace gave us a valid array for
-                 * plane information.
-                 */
-                ret = __verify_planes_array(vb, b);
-                if (ret)
-                        return ret;
-
                 /* Fill in driver-provided information for OUTPUT types */
                 if (V4L2_TYPE_IS_OUTPUT(b->type)) {
                         /*
@@ -872,8 +864,6 @@ static int __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b,
         vb->v4l2_buf.field = b->field;
         vb->v4l2_buf.timestamp = b->timestamp;
         vb->v4l2_buf.flags = b->flags & ~V4L2_BUFFER_STATE_FLAGS;
-
-        return 0;
 }
 
 /**
@@ -888,10 +878,8 @@ static int __qbuf_userptr(struct vb2_buffer *vb, const struct v4l2_buffer *b)
         int ret;
         int write = !V4L2_TYPE_IS_OUTPUT(q->type);
 
-        /* Verify and copy relevant information provided by the userspace */
-        ret = __fill_vb2_buffer(vb, b, planes);
-        if (ret)
-                return ret;
+        /* Copy relevant information provided by the userspace */
+        __fill_vb2_buffer(vb, b, planes);
 
         for (plane = 0; plane < vb->num_planes; ++plane) {
                 /* Skip the plane if already verified */
@@ -966,7 +954,8 @@ err:
  */
 static int __qbuf_mmap(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 {
-        return __fill_vb2_buffer(vb, b, vb->v4l2_planes);
+        __fill_vb2_buffer(vb, b, vb->v4l2_planes);
+        return 0;
 }
 
 /**
@@ -1059,7 +1048,9 @@ int vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b)
                 dprintk(1, "%s(): invalid buffer state %d\n", __func__, vb->state);
                 return -EINVAL;
         }
-
+        ret = __verify_planes_array(vb, b);
+        if (ret < 0)
+                return ret;
         ret = __buf_prepare(vb, b);
         if (ret < 0)
                 return ret;
@@ -1147,6 +1138,9 @@ int vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
                 ret = -EINVAL;
                 goto unlock;
         }
+        ret = __verify_planes_array(vb, b);
+        if (ret)
+                goto unlock;
 
         switch (vb->state) {
         case VB2_BUF_STATE_DEQUEUED:
@@ -1243,8 +1237,10 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
                  * the locks or return an error if one occurred.
                  */
                 call_qop(q, wait_finish, q);
-                if (ret)
+                if (ret) {
+                        dprintk(1, "Sleep was interrupted\n");
                         return ret;
+                }
         }
         return 0;
 }
@@ -1255,7 +1251,7 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
  * Will sleep if required for nonblocking == false.
  */
 static int __vb2_get_done_vb(struct vb2_queue *q, struct vb2_buffer **vb,
-                                int nonblocking)
+                                struct v4l2_buffer *b, int nonblocking)
 {
         unsigned long flags;
         int ret;
@@ -1273,10 +1269,16 @@ static int __vb2_get_done_vb(struct vb2_queue *q, struct vb2_buffer **vb,
          */
         spin_lock_irqsave(&q->done_lock, flags);
         *vb = list_first_entry(&q->done_list, struct vb2_buffer, done_entry);
-        list_del(&(*vb)->done_entry);
+        /*
+         * Only remove the buffer from done_list if v4l2_buffer can handle all
+         * the planes.
+         */
+        ret = __verify_planes_array(*vb, b);
+        if (!ret)
+                list_del(&(*vb)->done_entry);
         spin_unlock_irqrestore(&q->done_lock, flags);
 
-        return 0;
+        return ret;
 }
 
 /**
@@ -1335,12 +1337,9 @@ int vb2_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool nonblocking)
                 dprintk(1, "dqbuf: invalid buffer type\n");
                 return -EINVAL;
         }
-
-        ret = __vb2_get_done_vb(q, &vb, nonblocking);
-        if (ret < 0) {
-                dprintk(1, "dqbuf: error getting next done buffer\n");
+        ret = __vb2_get_done_vb(q, &vb, b, nonblocking);
+        if (ret < 0)
                 return ret;
-        }
 
         ret = call_qop(q, buf_finish, vb);
         if (ret) {
diff --git a/include/linux/dvb/version.h b/include/linux/dvb/version.h
index 20e5eac2ffd3..827cce7e33e3 100644
--- a/include/linux/dvb/version.h
+++ b/include/linux/dvb/version.h
@@ -24,6 +24,6 @@
 #define _DVBVERSION_H_
 
 #define DVB_API_VERSION 5
-#define DVB_API_VERSION_MINOR 8
+#define DVB_API_VERSION_MINOR 9
 
 #endif /*_DVBVERSION_H_*/
diff --git a/include/linux/platform_data/mipi-csis.h b/include/linux/platform_data/mipi-csis.h
index c45b1e8d4c2e..bf34e17cee7f 100644
--- a/include/linux/platform_data/mipi-csis.h
+++ b/include/linux/platform_data/mipi-csis.h
@@ -1,7 +1,7 @@
 /*
- * Copyright (C) 2010-2011 Samsung Electronics Co., Ltd.
+ * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
  *
- * S5P series MIPI CSI slave device support
+ * Samsung S5P/Exynos SoC series MIPI CSIS device support
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -11,33 +11,27 @@
 #ifndef __PLAT_SAMSUNG_MIPI_CSIS_H_
 #define __PLAT_SAMSUNG_MIPI_CSIS_H_ __FILE__
 
-struct platform_device;
-
 /**
  * struct s5p_platform_mipi_csis - platform data for S5P MIPI-CSIS driver
- * @clk_rate: bus clock frequency
- * @lanes: number of data lanes used
- * @alignment: data alignment in bits
- * @hs_settle: HS-RX settle time
- * @fixed_phy_vdd: false to enable external D-PHY regulator management in the
- *                 driver or true in case this regulator has no enable function
- * @phy_enable: pointer to a callback controlling D-PHY enable/reset
+ * @clk_rate:    bus clock frequency
+ * @wclk_source: CSI wrapper clock selection: 0 - bus clock, 1 - ext. SCLK_CAM
+ * @lanes:       number of data lanes used
+ * @hs_settle:   HS-RX settle time
  */
 struct s5p_platform_mipi_csis {
         unsigned long clk_rate;
+        u8 wclk_source;
         u8 lanes;
-        u8 alignment;
         u8 hs_settle;
-        bool fixed_phy_vdd;
-        int (*phy_enable)(struct platform_device *pdev, bool on);
 };
 
 /**
  * s5p_csis_phy_enable - global MIPI-CSI receiver D-PHY control
- * @pdev: MIPI-CSIS platform device
- * @on: true to enable D-PHY and deassert its reset
- *      false to disable D-PHY
+ * @id:     MIPI-CSIS harware instance index (0...1)
+ * @on:     true to enable D-PHY and deassert its reset
+ *          false to disable D-PHY
+ * @return: 0 on success, or negative error code on failure
  */
-int s5p_csis_phy_enable(struct platform_device *pdev, bool on);
+int s5p_csis_phy_enable(int id, bool on);
 
 #endif /* __PLAT_SAMSUNG_MIPI_CSIS_H_ */
diff --git a/include/linux/v4l2-controls.h b/include/linux/v4l2-controls.h
index 421d24c7f686..f56c945cecd4 100644
--- a/include/linux/v4l2-controls.h
+++ b/include/linux/v4l2-controls.h
@@ -349,6 +349,7 @@ enum v4l2_mpeg_video_multi_slice_mode {
 #define V4L2_CID_MPEG_VIDEO_VBV_SIZE                    (V4L2_CID_MPEG_BASE+222)
 #define V4L2_CID_MPEG_VIDEO_DEC_PTS                     (V4L2_CID_MPEG_BASE+223)
 #define V4L2_CID_MPEG_VIDEO_DEC_FRAME                   (V4L2_CID_MPEG_BASE+224)
+#define V4L2_CID_MPEG_VIDEO_VBV_DELAY                   (V4L2_CID_MPEG_BASE+225)
 
 #define V4L2_CID_MPEG_VIDEO_H263_I_FRAME_QP             (V4L2_CID_MPEG_BASE+300)
 #define V4L2_CID_MPEG_VIDEO_H263_P_FRAME_QP             (V4L2_CID_MPEG_BASE+301)
@@ -439,6 +440,46 @@ enum v4l2_mpeg_video_h264_vui_sar_idc {
         V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_2x1            = 16,
         V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_EXTENDED       = 17,
 };
+#define V4L2_CID_MPEG_VIDEO_H264_SEI_FRAME_PACKING              (V4L2_CID_MPEG_BASE+368)
+#define V4L2_CID_MPEG_VIDEO_H264_SEI_FP_CURRENT_FRAME_0         (V4L2_CID_MPEG_BASE+369)
+#define V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE        (V4L2_CID_MPEG_BASE+370)
+enum v4l2_mpeg_video_h264_sei_fp_arrangement_type {
+        V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_CHECKERBOARD       = 0,
+        V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_COLUMN             = 1,
+        V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_ROW                = 2,
+        V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_SIDE_BY_SIDE       = 3,
+        V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_TOP_BOTTOM         = 4,
+        V4L2_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE_TEMPORAL           = 5,
+};
+#define V4L2_CID_MPEG_VIDEO_H264_FMO                    (V4L2_CID_MPEG_BASE+371)
+#define V4L2_CID_MPEG_VIDEO_H264_FMO_MAP_TYPE           (V4L2_CID_MPEG_BASE+372)
+enum v4l2_mpeg_video_h264_fmo_map_type {
+        V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_INTERLEAVED_SLICES            = 0,
+        V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_SCATTERED_SLICES              = 1,
+        V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_FOREGROUND_WITH_LEFT_OVER     = 2,
+        V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_BOX_OUT                       = 3,
+        V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_RASTER_SCAN                   = 4,
+        V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_WIPE_SCAN                     = 5,
+        V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_EXPLICIT                      = 6,
+};
+#define V4L2_CID_MPEG_VIDEO_H264_FMO_SLICE_GROUP        (V4L2_CID_MPEG_BASE+373)
+#define V4L2_CID_MPEG_VIDEO_H264_FMO_CHANGE_DIRECTION   (V4L2_CID_MPEG_BASE+374)
+enum v4l2_mpeg_video_h264_fmo_change_dir {
+        V4L2_MPEG_VIDEO_H264_FMO_CHANGE_DIR_RIGHT       = 0,
+        V4L2_MPEG_VIDEO_H264_FMO_CHANGE_DIR_LEFT        = 1,
+};
+#define V4L2_CID_MPEG_VIDEO_H264_FMO_CHANGE_RATE        (V4L2_CID_MPEG_BASE+375)
+#define V4L2_CID_MPEG_VIDEO_H264_FMO_RUN_LENGTH         (V4L2_CID_MPEG_BASE+376)
+#define V4L2_CID_MPEG_VIDEO_H264_ASO                    (V4L2_CID_MPEG_BASE+377)
+#define V4L2_CID_MPEG_VIDEO_H264_ASO_SLICE_ORDER        (V4L2_CID_MPEG_BASE+378)
+#define V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING            (V4L2_CID_MPEG_BASE+379)
+#define V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_TYPE       (V4L2_CID_MPEG_BASE+380)
+enum v4l2_mpeg_video_h264_hierarchical_coding_type {
+        V4L2_MPEG_VIDEO_H264_HIERARCHICAL_CODING_B      = 0,
+        V4L2_MPEG_VIDEO_H264_HIERARCHICAL_CODING_P      = 1,
+};
+#define V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_LAYER      (V4L2_CID_MPEG_BASE+381)
+#define V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_LAYER_QP   (V4L2_CID_MPEG_BASE+382)
 #define V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP    (V4L2_CID_MPEG_BASE+400)
 #define V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP    (V4L2_CID_MPEG_BASE+401)
 #define V4L2_CID_MPEG_VIDEO_MPEG4_B_FRAME_QP    (V4L2_CID_MPEG_BASE+402)
@@ -757,5 +798,6 @@ enum v4l2_jpeg_chroma_subsampling {
 
 #define V4L2_CID_LINK_FREQ                      (V4L2_CID_IMAGE_PROC_CLASS_BASE + 1)
 #define V4L2_CID_PIXEL_RATE                     (V4L2_CID_IMAGE_PROC_CLASS_BASE + 2)
+#define V4L2_CID_TEST_PATTERN                   (V4L2_CID_IMAGE_PROC_CLASS_BASE + 3)
 
 #endif
diff --git a/include/linux/v4l2-mediabus.h b/include/linux/v4l2-mediabus.h
index 5ea7f753a348..7d64e0e1a18b 100644
--- a/include/linux/v4l2-mediabus.h
+++ b/include/linux/v4l2-mediabus.h
@@ -92,6 +92,11 @@ enum v4l2_mbus_pixelcode {
 
         /* JPEG compressed formats - next is 0x4002 */
         V4L2_MBUS_FMT_JPEG_1X8 = 0x4001,
+
+        /* Vendor specific formats - next is 0x5002 */
+
+        /* S5C73M3 sensor specific interleaved UYVY and JPEG */
+        V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8 = 0x5001,
 };
 
 /**
diff --git a/include/linux/videodev2.h b/include/linux/videodev2.h
index 61395ef85a00..873adbe82988 100644
--- a/include/linux/videodev2.h
+++ b/include/linux/videodev2.h
@@ -366,7 +366,9 @@ struct v4l2_pix_format {
 
 /* two non contiguous planes - one Y, one Cr + Cb interleaved  */
 #define V4L2_PIX_FMT_NV12M   v4l2_fourcc('N', 'M', '1', '2') /* 12  Y/CbCr 4:2:0  */
+#define V4L2_PIX_FMT_NV21M   v4l2_fourcc('N', 'M', '2', '1') /* 21  Y/CrCb 4:2:0  */
 #define V4L2_PIX_FMT_NV12MT  v4l2_fourcc('T', 'M', '1', '2') /* 12  Y/CbCr 4:2:0 64x32 macroblocks */
+#define V4L2_PIX_FMT_NV12MT_16X16 v4l2_fourcc('V', 'M', '1', '2') /* 12  Y/CbCr 4:2:0 16x16 macroblocks */
 
 /* three non contiguous planes - Y, Cb, Cr */
 #define V4L2_PIX_FMT_YUV420M v4l2_fourcc('Y', 'M', '1', '2') /* 12  YUV420 planar */
@@ -403,6 +405,7 @@ struct v4l2_pix_format {
 #define V4L2_PIX_FMT_MPEG     v4l2_fourcc('M', 'P', 'E', 'G') /* MPEG-1/2/4 Multiplexed */
 #define V4L2_PIX_FMT_H264     v4l2_fourcc('H', '2', '6', '4') /* H264 with start codes */
 #define V4L2_PIX_FMT_H264_NO_SC v4l2_fourcc('A', 'V', 'C', '1') /* H264 without start codes */
+#define V4L2_PIX_FMT_H264_MVC v4l2_fourcc('M', '2', '6', '4') /* H264 MVC */
 #define V4L2_PIX_FMT_H263     v4l2_fourcc('H', '2', '6', '3') /* H263          */
 #define V4L2_PIX_FMT_MPEG1    v4l2_fourcc('M', 'P', 'G', '1') /* MPEG-1 ES     */
 #define V4L2_PIX_FMT_MPEG2    v4l2_fourcc('M', 'P', 'G', '2') /* MPEG-2 ES     */
@@ -410,6 +413,7 @@ struct v4l2_pix_format {
 #define V4L2_PIX_FMT_XVID     v4l2_fourcc('X', 'V', 'I', 'D') /* Xvid           */
 #define V4L2_PIX_FMT_VC1_ANNEX_G v4l2_fourcc('V', 'C', '1', 'G') /* SMPTE 421M Annex G compliant stream */
 #define V4L2_PIX_FMT_VC1_ANNEX_L v4l2_fourcc('V', 'C', '1', 'L') /* SMPTE 421M Annex L compliant stream */
+#define V4L2_PIX_FMT_VP8      v4l2_fourcc('V', 'P', '8', '0') /* VP8 */
 
 /*  Vendor-specific formats   */
 #define V4L2_PIX_FMT_CPIA1    v4l2_fourcc('C', 'P', 'I', 'A') /* cpia1 YUV */
@@ -437,6 +441,7 @@ struct v4l2_pix_format {
 #define V4L2_PIX_FMT_KONICA420  v4l2_fourcc('K', 'O', 'N', 'I') /* YUV420 planar in blocks of 256 pixels */
 #define V4L2_PIX_FMT_JPGL       v4l2_fourcc('J', 'P', 'G', 'L') /* JPEG-Lite */
 #define V4L2_PIX_FMT_SE401      v4l2_fourcc('S', '4', '0', '1') /* se401 janggu compressed rgb */
+#define V4L2_PIX_FMT_S5C_UYVY_JPG v4l2_fourcc('S', '5', 'C', 'I') /* S5C73M3 interleaved UYVY/JPEG */
 
 /*
  *      F O R M A T   E N U M E R A T I O N
diff --git a/include/media/davinci/vpbe.h b/include/media/davinci/vpbe.h
index 8bc1b3c0e679..a7ca4884c46c 100644
--- a/include/media/davinci/vpbe.h
+++ b/include/media/davinci/vpbe.h
@@ -35,7 +35,7 @@ struct osd_config_info {
 struct vpbe_output {
         struct v4l2_output output;
         /*
-         * If output capabilities include dv_preset, list supported presets
+         * If output capabilities include dv_timings, list supported timings
          * below
          */
         char *subdev_name;
@@ -120,16 +120,16 @@ struct vpbe_device_ops {
         unsigned int (*get_output)(struct vpbe_device *vpbe_dev);
 
         /* Set DV preset at current output */
-        int (*s_dv_preset)(struct vpbe_device *vpbe_dev,
-                           struct v4l2_dv_preset *dv_preset);
+        int (*s_dv_timings)(struct vpbe_device *vpbe_dev,
+                           struct v4l2_dv_timings *dv_timings);
 
         /* Get DV presets supported at the output */
-        int (*g_dv_preset)(struct vpbe_device *vpbe_dev,
-                           struct v4l2_dv_preset *dv_preset);
+        int (*g_dv_timings)(struct vpbe_device *vpbe_dev,
+                           struct v4l2_dv_timings *dv_timings);
 
         /* Enumerate the DV Presets supported at the output */
-        int (*enum_dv_presets)(struct vpbe_device *vpbe_dev,
-                               struct v4l2_dv_enum_preset *preset_info);
+        int (*enum_dv_timings)(struct vpbe_device *vpbe_dev,
+                               struct v4l2_enum_dv_timings *timings_info);
 
         /* Set std at the output */
         int (*s_std)(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id);
diff --git a/include/media/davinci/vpbe_types.h b/include/media/davinci/vpbe_types.h
index 727f55170e41..9b85396514be 100644
--- a/include/media/davinci/vpbe_types.h
+++ b/include/media/davinci/vpbe_types.h
@@ -32,11 +32,6 @@ enum vpbe_enc_timings_type {
         VPBE_ENC_TIMINGS_INVALID = 0x8,
 };
 
-union vpbe_timings {
-        v4l2_std_id std_id;
-        unsigned int dv_preset;
-};
-
 /*
  * struct vpbe_enc_mode_info
  * @name: ptr to name string of the standard, "NTSC", "PAL" etc
@@ -73,7 +68,8 @@ union vpbe_timings {
 struct vpbe_enc_mode_info {
         unsigned char *name;
         enum vpbe_enc_timings_type timings_type;
-        union vpbe_timings timings;
+        v4l2_std_id std_id;
+        struct v4l2_dv_timings dv_timings;
         unsigned int interlaced;
         unsigned int xres;
         unsigned int yres;
diff --git a/include/media/davinci/vpbe_venc.h b/include/media/davinci/vpbe_venc.h
index 6b57334f4029..cc78c2eb16da 100644
--- a/include/media/davinci/vpbe_venc.h
+++ b/include/media/davinci/vpbe_venc.h
@@ -32,7 +32,7 @@ struct venc_platform_data {
         int (*setup_pinmux)(enum v4l2_mbus_pixelcode if_type,
                             int field);
         int (*setup_clock)(enum vpbe_enc_timings_type type,
-                           unsigned int mode);
+                           unsigned int pixclock);
         int (*setup_if_config)(enum v4l2_mbus_pixelcode pixcode);
         /* Number of LCD outputs supported */
         int num_lcd_outputs;
diff --git a/include/media/davinci/vpif_types.h b/include/media/davinci/vpif_types.h
index d8f6ab1943e4..3882e0675ccf 100644
--- a/include/media/davinci/vpif_types.h
+++ b/include/media/davinci/vpif_types.h
@@ -20,6 +20,7 @@
 #include <linux/i2c.h>
 
 #define VPIF_CAPTURE_MAX_CHANNELS       2
+#define VPIF_DISPLAY_MAX_CHANNELS       2
 
 enum vpif_if_type {
         VPIF_IF_BT656,
@@ -37,29 +38,38 @@ struct vpif_interface {
 struct vpif_subdev_info {
         const char *name;
         struct i2c_board_info board_info;
-        u32 input;
-        u32 output;
-        unsigned can_route:1;
-        struct vpif_interface vpif_if;
+};
+
+struct vpif_output {
+        struct v4l2_output output;
+        const char *subdev_name;
+        u32 input_route;
+        u32 output_route;
+};
+
+struct vpif_display_chan_config {
+        const struct vpif_output *outputs;
+        int output_count;
+        bool clip_en;
 };
 
 struct vpif_display_config {
         int (*set_clock)(int, int);
         struct vpif_subdev_info *subdevinfo;
         int subdev_count;
-        const char **output;
-        int output_count;
+        struct vpif_display_chan_config chan_config[VPIF_DISPLAY_MAX_CHANNELS];
         const char *card_name;
-        bool ch2_clip_en;
-        bool ch3_clip_en;
 };
 
 struct vpif_input {
         struct v4l2_input input;
         const char *subdev_name;
+        u32 input_route;
+        u32 output_route;
 };
 
 struct vpif_capture_chan_config {
+        struct vpif_interface vpif_if;
         const struct vpif_input *inputs;
         int input_count;
 };
diff --git a/include/media/s5p_fimc.h b/include/media/s5p_fimc.h
index 09421a611d73..eaea62a382f8 100644
--- a/include/media/s5p_fimc.h
+++ b/include/media/s5p_fimc.h
@@ -30,7 +30,6 @@ struct i2c_board_info;
  * @board_info: pointer to I2C subdevice's board info
  * @clk_frequency: frequency of the clock the host interface provides to sensor
  * @bus_type: determines bus type, MIPI, ITU-R BT.601 etc.
- * @csi_data_align: MIPI-CSI interface data alignment in bits
  * @i2c_bus_num: i2c control bus id the sensor is attached to
  * @mux_id: FIMC camera interface multiplexer index (separate for MIPI and ITU)
  * @clk_id: index of the SoC peripheral clock for sensors
@@ -40,7 +39,6 @@ struct s5p_fimc_isp_info {
         struct i2c_board_info *board_info;
         unsigned long clk_frequency;
         enum cam_bus_type bus_type;
-        u16 csi_data_align;
         u16 i2c_bus_num;
         u16 mux_id;
         u16 flags;
diff --git a/include/media/v4l2-ctrls.h b/include/media/v4l2-ctrls.h
index 801adb466bd2..96509119f28f 100644
--- a/include/media/v4l2-ctrls.h
+++ b/include/media/v4l2-ctrls.h
@@ -351,6 +351,29 @@ struct v4l2_ctrl *v4l2_ctrl_new_std_menu(struct v4l2_ctrl_handler *hdl,
                         const struct v4l2_ctrl_ops *ops,
                         u32 id, s32 max, s32 mask, s32 def);
 
+/** v4l2_ctrl_new_std_menu_items() - Create a new standard V4L2 menu control
+  * with driver specific menu.
+  * @hdl:       The control handler.
+  * @ops:       The control ops.
+  * @id:        The control ID.
+  * @max:       The control's maximum value.
+  * @mask:      The control's skip mask for menu controls. This makes it
+  *             easy to skip menu items that are not valid. If bit X is set,
+  *             then menu item X is skipped. Of course, this only works for
+  *             menus with <= 32 menu items. There are no menus that come
+  *             close to that number, so this is OK. Should we ever need more,
+  *             then this will have to be extended to a u64 or a bit array.
+  * @def:       The control's default value.
+  * @qmenu:     The new menu.
+  *
+  * Same as v4l2_ctrl_new_std_menu(), but @qmenu will be the driver specific
+  * menu of this control.
+  *
+  */
+struct v4l2_ctrl *v4l2_ctrl_new_std_menu_items(struct v4l2_ctrl_handler *hdl,
+                        const struct v4l2_ctrl_ops *ops, u32 id, s32 max,
+                        s32 mask, s32 def, const char * const *qmenu);
+
 /** v4l2_ctrl_new_int_menu() - Create a new standard V4L2 integer menu control.
   * @hdl:       The control handler.
   * @ops:       The control ops.
diff --git a/include/media/v4l2-subdev.h b/include/media/v4l2-subdev.h
index 2ecd7377153b..b137a5e1151a 100644
--- a/include/media/v4l2-subdev.h
+++ b/include/media/v4l2-subdev.h
@@ -21,6 +21,7 @@
 #ifndef _V4L2_SUBDEV_H
 #define _V4L2_SUBDEV_H
 
+#include <linux/types.h>
 #include <linux/v4l2-subdev.h>
 #include <media/media-entity.h>
 #include <media/v4l2-common.h>
@@ -45,6 +46,7 @@ struct v4l2_fh;
 struct v4l2_subdev;
 struct v4l2_subdev_fh;
 struct tuner_setup;
+struct v4l2_mbus_frame_desc;
 
 /* decode_vbi_line */
 struct v4l2_decode_vbi_line {
@@ -226,6 +228,36 @@ struct v4l2_subdev_audio_ops {
         int (*s_stream)(struct v4l2_subdev *sd, int enable);
 };
 
+/* Indicates the @length field specifies maximum data length. */
+#define V4L2_MBUS_FRAME_DESC_FL_LEN_MAX         (1U << 0)
+/* Indicates user defined data format, i.e. non standard frame format. */
+#define V4L2_MBUS_FRAME_DESC_FL_BLOB            (1U << 1)
+
+/**
+ * struct v4l2_mbus_frame_desc_entry - media bus frame description structure
+ * @flags: V4L2_MBUS_FRAME_DESC_FL_* flags
+ * @pixelcode: media bus pixel code, valid if FRAME_DESC_FL_BLOB is not set
+ * @length: number of octets per frame, valid for compressed or unspecified
+ *          formats
+ */
+struct v4l2_mbus_frame_desc_entry {
+        u16 flags;
+        u32 pixelcode;
+        u32 length;
+};
+
+#define V4L2_FRAME_DESC_ENTRY_MAX       4
+
+/**
+ * struct v4l2_mbus_frame_desc - media bus data frame description
+ * @entry: frame descriptors array
+ * @num_entries: number of entries in @entry array
+ */
+struct v4l2_mbus_frame_desc {
+        struct v4l2_mbus_frame_desc_entry entry[V4L2_FRAME_DESC_ENTRY_MAX];
+        unsigned short num_entries;
+};
+
 /*
    s_std_output: set v4l2_std_id for video OUTPUT devices. This is ignored by
         video input devices.
@@ -274,6 +306,10 @@ struct v4l2_subdev_audio_ops {
    s_mbus_config: set a certain mediabus configuration. This operation is added
         for compatibility with soc-camera drivers and should not be used by new
         software.
+
+   s_rx_buffer: set a host allocated memory buffer for the subdev. The subdev
+        can adjust @size to a lower value and must not write more data to the
+        buffer starting at @data than the original value of @size.
  */
 struct v4l2_subdev_video_ops {
         int (*s_routing)(struct v4l2_subdev *sd, u32 input, u32 output, u32 config);
@@ -327,6 +363,8 @@ struct v4l2_subdev_video_ops {
                              struct v4l2_mbus_config *cfg);
         int (*s_mbus_config)(struct v4l2_subdev *sd,
                              const struct v4l2_mbus_config *cfg);
+        int (*s_rx_buffer)(struct v4l2_subdev *sd, void *buf,
+                           unsigned int *size);
 };
 
 /*
@@ -455,6 +493,12 @@ struct v4l2_subdev_ir_ops {
                                 struct v4l2_subdev_ir_parameters *params);
 };
 
+/**
+ * struct v4l2_subdev_pad_ops - v4l2-subdev pad level operations
+ * @get_frame_desc: get the current low level media bus frame parameters.
+ * @get_frame_desc: set the low level media bus frame parameters, @fd array
+ *                  may be adjusted by the subdev driver to device capabilities.
+ */
 struct v4l2_subdev_pad_ops {
         int (*enum_mbus_code)(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
                               struct v4l2_subdev_mbus_code_enum *code);
@@ -483,6 +527,10 @@ struct v4l2_subdev_pad_ops {
                              struct v4l2_subdev_format *source_fmt,
                              struct v4l2_subdev_format *sink_fmt);
 #endif /* CONFIG_MEDIA_CONTROLLER */
+        int (*get_frame_desc)(struct v4l2_subdev *sd, unsigned int pad,
+                              struct v4l2_mbus_frame_desc *fd);
+        int (*set_frame_desc)(struct v4l2_subdev *sd, unsigned int pad,
+                              struct v4l2_mbus_frame_desc *fd);
 };
 
 struct v4l2_subdev_ops {